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

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