sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add land service

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-08 21:21:12 +02:00
parent ca2049b0db
commit 4daf79cdc0
4 changed files with 19 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
api/views/index.ejs
View File

@@ -65,6 +65,7 @@
<p id="control_mode"></p>
<p id="speed">Current speed</p>
<p id="position">Current position</p>
<p id="height">Height</p>
<p id="failsafe">Failsafe not activated</p>
<img class="headerimg"
src="https://upload.wikimedia.org/wikipedia/commons/thumb/e/e9/Ericsson_logo.svg/2341px-Ericsson_logo.svg.png"
@@ -139,6 +140,7 @@
document.getElementById("control_mode").innerHTML = "Control mode: " + data.data.control_mode;
document.getElementById("speed").innerHTML = "Current speed (m/s): x: " + data.data.velocity[0] + " y: " + data.data.velocity[1] + " z: " + data.data.velocity[2];
document.getElementById("position").innerHTML = "Current position (m): x: " + data.data.position[0] + " y: " + data.data.position[1] + " z: " + data.data.position[2];
document.getElementById("height").innerHTML = "Height (m): " + data.data.height;
} else if (data.type == "FAILSAFE") {
document.getElementById("failsafe").innerHTML = "Failsafe: ACTIVATED";
document.getElementById("failsafe").style.backgroundColor = "red";
@@ -313,8 +315,8 @@
document.getElementById("cpuload").innerHTML = "CPU load: " + data.data.cpu_usage.toString().substring(0, 6).substring(2, 4) + "%";
document.getElementById("armed").innerHTML = "Armed: " + data.data.armed;
document.getElementById("control_mode").innerHTML = "Control mode: " + data.data.control_mode;
document.getElementById("speed").innerHTML = "Current speed (m/s): x: " + data.data.velocity[0] + " y: " + data.data.velocity[1] + " z: " + data.data.velocity[2];
document.getElementById("position").innerHTML = "Current position (m): x: " + data.data.position[0] + " y: " + data.data.position[1] + " z: " + data.data.position[2];
document.getElementById("speed").innerHTML = "Current speed (m/s): x: " + data.data.velocity[0].toString().substring(0,4) + " y: " + data.data.velocity[1].toString().substring(0,4) + " z: " + data.data.velocity[2].toString().substring(0,4);
document.getElementById("position").innerHTML = "Current position (m): x: " + data.data.position[0].toString().substring(0,4) + " y: " + data.data.position[1].toString().substring(0,4) + " z: " + data.data.position[2].toString().substring(0,4);
// TODO fix
// if (set_timout == false) {
// api_timeout = setTimeout(function () {

9
src/api_communication/api_communication/api_listener.py
View File

@@ -153,11 +153,12 @@ class ApiListener(Node):
self.armed = msg.armed
self.status_data['control_mode'] = msg.control_mode
self.status_data['route_setpoint'] = msg.route_setpoint
self.status_data['velocity'] = [int(msg.velocity[0]), int(
msg.velocity[1]), int(msg.velocity[2])]
self.status_data['position'] = [int(msg.position[0]), int(
msg.position[1]), int(msg.position[2])]
self.status_data['velocity'] = [float(msg.velocity[0]), float(
msg.velocity[1]), float(msg.velocity[2])]
self.status_data['position'] = [float(msg.position[0]), float(
msg.position[1]), float(msg.position[2])]
self.status_data['failsafe'] = msg.failsafe
self.status_data['height'] = msg.height
except Exception as e:
self.get_logger().error(
f'Error while parsing drone status message: {e}')

8
src/drone_controls/src/PositionChanger.cpp
View File

@@ -49,6 +49,7 @@ public:
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
this->lidar_subscription = this->create_subscription<drone_services::msg::LidarReading>("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this, std::placeholders::_1));
this->height_subscription = this->create_subscription<drone_services::msg::HeightData>("/drone/height", qos, std::bind(&PositionChanger::handle_height_message, this, std::placeholders::_1));
// vehicle_local_position_subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1));
this->odometry_subscription = this->create_subscription<px4_msgs::msg::VehicleOdometry>("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1));
this->move_position_service = this->create_service<drone_services::srv::MovePosition>("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
@@ -270,6 +271,11 @@ public:
apply_collision_weights();
}
void handle_height_message(const drone_services::msg::HeightData::SharedPtr message)
{
this->current_height = message->height;
}
/**
* @brief Callback function for receiving new LIDAR data
*
@@ -462,6 +468,8 @@ private:
float current_speed_x = 0;
float current_speed_y = 0;
float current_speed_z = 0;
float current_height = 0;
bool is_landing = false;
bool move_direction_allowed[4] = {true}; // whether the drone can move in a certain direction
float collision_prevention_weights[4] = {0}; // the amount to move away from an object in a certain direction if the drone is too close
bool failsafe_enabled = false;

2
src/drone_services/srv/Land.srv Normal file
View File

@@ -0,0 +1,2 @@
---
bool is_landing