From 3da7ac030611a776b653bd7f2f94290b701024a5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 12:52:33 +0200 Subject: [PATCH] change service response to success boolean --- .vscode/c_cpp_properties.json | 3 ++- src/drone_services/srv/SetAttitude.srv | 2 +- src/drone_services/srv/SetVehicleControl.srv | 2 +- src/drone_services/srv/SetVelocity.srv | 2 +- src/px4_connection/src/heartbeat.cpp | 4 ++-- src/px4_connection/src/px4_controller.cpp | 18 +++++++++++++++--- 6 files changed, 22 insertions(+), 9 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 6743b427..b8851067 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -12,7 +12,8 @@ "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/rtls_driver/include/**", "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/serial_communication/include/**", "/mnt/Homework/Avans/AFSTUDEERSTAGE/terabee_api/include/**", - "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**" + "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**", + "/home/ubuntu/ros2_ws/src/px4_connection/include/**" ], "name": "ROS", "intelliSenseMode": "gcc-x64", diff --git a/src/drone_services/srv/SetAttitude.srv b/src/drone_services/srv/SetAttitude.srv index 52957c0c..9a8125d7 100644 --- a/src/drone_services/srv/SetAttitude.srv +++ b/src/drone_services/srv/SetAttitude.srv @@ -6,4 +6,4 @@ float32 roll #thrust between -1 and 1 float32 thrust --- -int8 status +bool success diff --git a/src/drone_services/srv/SetVehicleControl.srv b/src/drone_services/srv/SetVehicleControl.srv index d96ce97a..6ae9ec4c 100644 --- a/src/drone_services/srv/SetVehicleControl.srv +++ b/src/drone_services/srv/SetVehicleControl.srv @@ -12,4 +12,4 @@ # 32: position int32 control # control bitmask --- -int8 status # status of operation \ No newline at end of file +bool success # if operation succeeded \ No newline at end of file diff --git a/src/drone_services/srv/SetVelocity.srv b/src/drone_services/srv/SetVelocity.srv index 0d85cba9..6602ab3e 100644 --- a/src/drone_services/srv/SetVelocity.srv +++ b/src/drone_services/srv/SetVelocity.srv @@ -6,4 +6,4 @@ float32 z_speed #angle is in degrees float32 angle --- -int8 status +bool success diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 9c4bdd04..4e5795c5 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -109,14 +109,14 @@ private: { if (request->control < 0 || request->control > (1 << CONTROL_POSITION_POS)) { - response->status = 1; + response->success = false; } else { this->control_mode = request->control; RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode); publish_new_control_mode(); - response->status = 0; + response->success = true; } } }; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 250c0f60..f4b561fc 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -42,7 +43,8 @@ public: // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1)); - + control_mode_subscription_ = this->create_subscription("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1)); + set_attitude_service_ = this->create_service("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -58,10 +60,13 @@ private: rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; rclcpp::Subscription::SharedPtr vehicle_attitude_subscription_; + rclcpp::Subscription::SharedPtr control_mode_subscription_; rclcpp::Service::SharedPtr set_attitude_service_; rclcpp::Service::SharedPtr disarm_service_; + + // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -83,6 +88,8 @@ private: float base_q[4] = {0, 0, 0, 0}; int base_q_amount = 0; + char current_control_mode = CONTROL_MODE_ATTITUDE; + // result quaternion std::array q = {0, 0, 0, 0}; @@ -117,7 +124,7 @@ private: RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); new_setpoint = true; - response->status = 0; + response->success = true; } else { @@ -125,7 +132,7 @@ private: last_setpoint[0] = 0; last_setpoint[1] = 0; last_setpoint[2] = 0; - response->status = 1; + response->success = false; } } @@ -286,6 +293,11 @@ private: } } + void on_control_mode_receive(const drone_services::msg::DroneControlMode::SharedPtr msg) + { + + } + /** * @brief Publish vehicle commands * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes)