From dae0d06d61828cf94229cbc49ad59f8930726be2 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 11:55:36 +0200 Subject: [PATCH] update drone services msg definitions in positionchanger --- src/drone_controls/src/PositionChanger.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 50f96241..f8e0d803 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -1,8 +1,8 @@ #include "rclcpp/rclcpp.hpp" #include -#include -#include +#include +#include #include #include #include @@ -39,7 +39,7 @@ public: rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); - this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this, std::placeholders::_1)) this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); + this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this, std::placeholders::_1)) this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); this->move_position_service = this->create_service("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -110,7 +110,7 @@ check_move_direction_allowed() : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR])); } -void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) +void handle_lidar_message(const drone_services::msg::LidarReading::SharedPtr message) { if (message->sensor_1 > 0) @@ -185,7 +185,7 @@ static void get_x_y_with_rotation(float x, float y, float yaw, float *x_res, flo } private: -rclcpp::Subscription lidar_subscription; +rclcpp::Subscription lidar_subscription; rclcpp::Subscription odometry_subscription; rclcpp::Client::SharedPtr trajectory_client;