add service

src/drone_controls/src/PositionChanger.cpp

@@ -1,10 +1,11 @@
 #include "rclcpp/rclcpp.hpp"
 
-#include "px4_msgs/msg/VehicleOdometry.msg"
-#include "object_detection/msg/LidarReading.msg"
-#include "height/msg/HeightData.msg"
-#include "drone_services/srv/SetVehicleControl.srv"
-#include "drone_services/srv/SetTrajectory/srv"
+#include <px4_msgs/msg/vehicle_odometry.hpp>
+#include <object_detection/msg/lidar_reading.hpp>
+#include <height/msg/height_data.hpp>
+#include <drone_services/srv/set_vehicle_control.hpp>
+#include <drone_services/srv/set_trajectory.hpp>
+#include <drone_services/srv/move_position.hpp>
 
 #define _USE_MATH_DEFINES
 #include <cmath>
@@ -21,6 +22,8 @@
 
 #define MIN_DISTANCE 1.0 // in meters
 
+#define DEFAULT_CONTROL_MODE 4 // default velocity control bitmask
+
 using namespace std::chrono_literals;
 
 struct Quaternion
@@ -38,6 +41,7 @@ public:
 
         this->lidar_subscription = this->create_subscription<object_detection::msg::LidarReading>("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, 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->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_service::srv::MovePosition>("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
         this->trajectory_client = this->create_client<drone_services::srv::SetTrajectory>("/drone/set_trajectory");
         this->vehicle_control_client = this->create_client<drone_services::srv::SetVehicleControl>("/drone/set_vehicle_control");
@@ -46,6 +50,22 @@ public:
         wait_for_service<rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr>(this->trajectory_client);
         RCLCPP_INFO(this->get_logger(), "waiting for vehicle control service...");
         wait_for_service<rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr>(this->vehicle_control_client);
+
+        this->trajectory_request = std::make_shared<drone_services::srv::SetTrajectory::Request>();
+        this->vehicle_control_request = std::make_shared<drone_services::srv::SetVehicleControl::Request>();
+
+        this->vehicle_control_request->control_mode = DEFAULT_CONTROL_MODE;
+        auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request);
+
+        if (rclcpp::spin_until_future_complete(this, control_mode_response) ==
+            rclcpp::FutureReturnCode::SUCCESS)
+        {
+            RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", control_mode_response.get()->success);
+        }
+        else
+        {
+            RCLCPP_ERROR(this->get_logger(), "Failed to call service to set vehicle control");
+        }
     }
 }
 
@@ -53,7 +73,8 @@ public:
  * @brief checks for every direction is an object is too close and if we can move in that direction.
  * If the object is too close to the drone, calculate the amount we need to move away from it
  */
-    void check_move_direction_allowed()
+void
+check_move_direction_allowed()
 {
     this->move_direction_allowed[MOVE_DIRECTION_FRONT] = this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE;
     this->move_direction_allowed[MOVE_DIRECTION_LEFT] = this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE;
@@ -105,6 +126,16 @@ public:
     current_yaw = get_yaw_angle(q);
 }
 
+void handle_move_position_request(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<drone_services::srv::MovePosition::Request> request,
+    const std::shared_ptr<drone_services::srv::MovePosition::Response> response)
+{
+    //TODO add check_move_direction_allowed results to this calculation
+    this->current_yaw = request->angle * (M_PI / 180.0); // get the angle in radians
+    get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
+}
+
 /**
  * @brief Get the yaw angle from a specified normalized quaternion.
  * Uses the theory from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
@@ -128,17 +159,21 @@ public:
  * @param x_out the resulting x coordinate
  * @param y_out the resulting y coordinate
  */
-    static void get_x_y_with_rotation(float x, float y, float yaw, float &x_out, float &y_out)
+static void get_x_y_with_rotation(float x, float y, float yaw, float *x_res, float *y_res)
 {
-        x_out = x * std::cos(yaw) - y * std::sin(yaw);
-        y_out = x * std::sin(yaw) + y * std::cos(yaw);
+    *x_res = x * std::cos(yaw) - y * std::sin(yaw);
+    *y_res = x * std::sin(yaw) + y * std::cos(yaw);
 }
 
 private:
 rclcpp::Subscription<object_detection::msg::LidarReading> lidar_subscription;
 rclcpp::Subscription<px4_msgs::msg::VehicleOdometry> odometry_subscription;
+
 rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr trajectory_client;
 rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_client;
+
+rclcpp::Service<drone_services::srv::MovePosition>::SharedPtr move_position_service;
+
 drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request;
 drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request;
 
@@ -163,9 +198,9 @@ private:
         }
         RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
     }
-
 }
-};
+}
+;
 int main(int argc, char *argv[])
 {
     rclcpp::init(argc, argv);

src/drone_services/CMakeLists.txt

@@ -29,7 +29,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/ArmDrone.srv"
   "srv/DisarmDrone.srv"
   "srv/ControlRelais.srv"
+  "srv/MovePosition.srv"
   "msg/DroneControlMode.msg"
+  "msg/DroneArmStatus.msg"
+  "msg/DroneRouteStatus.msg"
+  "msg/DroneStatus.msg"
 )
 
 if(BUILD_TESTING)

src/drone_services/srv/MovePosition.srv

@@ -0,0 +1,6 @@
+float front_back
+float left_right
+float up_down
+float angle # in degrees
+---
+bool success