From 23c930cd06980ffe4c8cac024682385215f0a959 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 7 Jun 2024 09:42:22 +0200 Subject: [PATCH] reformat with jazzy's ament_uncrustify --- mavros/include/mavros/px4_custom_mode.hpp | 2 +- mavros/test/test_router.cpp | 2 +- mavros_extras/src/plugins/gimbal_control.cpp | 70 +++++++++++--------- mavros_extras/src/plugins/trajectory.cpp | 6 +- 4 files changed, 42 insertions(+), 38 deletions(-) diff --git a/mavros/include/mavros/px4_custom_mode.hpp b/mavros/include/mavros/px4_custom_mode.hpp index 3fb19338b..a8e664e30 100644 --- a/mavros/include/mavros/px4_custom_mode.hpp +++ b/mavros/include/mavros/px4_custom_mode.hpp @@ -100,7 +100,7 @@ union custom_mode { } constexpr custom_mode(uint8_t mm, uint8_t sm) - : mode{0, mm, sm} + : mode{0, mm, sm} { } }; diff --git a/mavros/test/test_router.cpp b/mavros/test/test_router.cpp index 335908e2d..1f68c16d1 100644 --- a/mavros/test/test_router.cpp +++ b/mavros/test/test_router.cpp @@ -93,7 +93,7 @@ class TestRouter : public ::testing::Test auto make_and_add_mock_endpoint = [router](id_t id, const std::string & url, LT type, - std::set remotes) { + std::set remotes) { auto ep = std::make_shared(); ep->parent = router; diff --git a/mavros_extras/src/plugins/gimbal_control.cpp b/mavros_extras/src/plugins/gimbal_control.cpp index 7afc9258f..12e22bd0a 100644 --- a/mavros_extras/src/plugins/gimbal_control.cpp +++ b/mavros_extras/src/plugins/gimbal_control.cpp @@ -108,13 +108,15 @@ class GimbalControlPlugin : public plugin::Plugin _1)); // --Not successfully validated-- - set_manager_attitude_sub = node->create_subscription( + set_manager_attitude_sub = + node->create_subscription( "~/manager/set_attitude", 10, std::bind( &GimbalControlPlugin::manager_set_attitude_cb, this, _1)); // --Not successfully validated-- - set_manager_pitchyaw_sub = node->create_subscription( + set_manager_pitchyaw_sub = + node->create_subscription( "~/manager/set_pitchyaw", 10, std::bind( &GimbalControlPlugin::manager_set_pitchyaw_cb, this, _1)); @@ -132,16 +134,16 @@ class GimbalControlPlugin : public plugin::Plugin 10); gimbal_manager_status_pub = node->create_publisher( - "~/manager/status", + "~/manager/status", 10); gimbal_manager_info_pub = node->create_publisher( - "~/manager/info", + "~/manager/info", 10); // --Not successfully validated-- gimbal_device_info_pub = node->create_publisher( - "~/device/info", + "~/device/info", 10); @@ -151,7 +153,7 @@ class GimbalControlPlugin : public plugin::Plugin "~/device/get_info", std::bind( &GimbalControlPlugin::device_get_info_cb, this, _1, _2)); - + gimbal_manager_info_srv = node->create_service( "~/manager/get_info", std::bind( &GimbalControlPlugin::manager_get_info_cb, @@ -177,11 +179,13 @@ class GimbalControlPlugin : public plugin::Plugin "~/manager/camera_track", std::bind( &GimbalControlPlugin::manager_camera_track, this, _1, _2)); - + // Client used by all services for sending mavros/cmd/command service calls, on a separate callback group to support nested service calls - cmdClient = node->create_client("cmd/command", rmw_qos_profile_services_default, cb_group); + cmdClient = node->create_client("cmd/command", + rmw_qos_profile_services_default, cb_group); while (!cmdClient->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_ERROR(node->get_logger(), "GimbalControl: mavros/cmd/command service not available after waiting"); + RCLCPP_ERROR(node->get_logger(), + "GimbalControl: mavros/cmd/command service not available after waiting"); } } @@ -200,12 +204,16 @@ class GimbalControlPlugin : public plugin::Plugin rclcpp::CallbackGroup::SharedPtr cb_group; // Subscribers rclcpp::Subscription::SharedPtr set_device_attitude_sub; - rclcpp::Subscription::SharedPtr set_manager_attitude_sub; - rclcpp::Subscription::SharedPtr set_manager_pitchyaw_sub; - rclcpp::Subscription::SharedPtr set_manager_manual_control_sub; + rclcpp::Subscription::SharedPtr + set_manager_attitude_sub; + rclcpp::Subscription::SharedPtr + set_manager_pitchyaw_sub; + rclcpp::Subscription::SharedPtr + set_manager_manual_control_sub; // Publishers - rclcpp::Publisher::SharedPtr gimbal_attitude_status_pub; + rclcpp::Publisher::SharedPtr + gimbal_attitude_status_pub; rclcpp::Publisher::SharedPtr gimbal_manager_status_pub; rclcpp::Publisher::SharedPtr gimbal_manager_info_pub; rclcpp::Publisher::SharedPtr gimbal_device_info_pub; @@ -435,7 +443,8 @@ class GimbalControlPlugin : public plugin::Plugin * Note that message contents is identical to that of pitchyaw, so the message type is re-used here * @param req - received GimbalControl msg */ - void manager_set_manual_control_cb(const mavros_msgs::msg::GimbalManagerSetPitchyaw::SharedPtr req) + void manager_set_manual_control_cb( + const mavros_msgs::msg::GimbalManagerSetPitchyaw::SharedPtr req) { mavlink::common::msg::GIMBAL_MANAGER_SET_PITCHYAW msg {}; uas->msg_set_target(msg); @@ -478,7 +487,9 @@ class GimbalControlPlugin : public plugin::Plugin get_logger(), !res->success, "GimbalControl: plugin service call failed!"); } - void device_get_info_response(rclcpp::Client::SharedFuture response) { + void device_get_info_response( + rclcpp::Client::SharedFuture response) + { auto result = response.get(); } @@ -575,7 +586,7 @@ class GimbalControlPlugin : public plugin::Plugin /** * @brief Set Gimbal ROI mode and parameters * - * Message specifications: + * Message specifications: * https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_LOCATION * https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET * https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_SYSID @@ -596,29 +607,25 @@ class GimbalControlPlugin : public plugin::Plugin cmdrq->param5 = req->latitude; cmdrq->param6 = req->longitude; cmdrq->param7 = req->altitude; - } - else if (req->mode == req->ROI_MODE_WP_NEXT_OFFSET) { + } else if (req->mode == req->ROI_MODE_WP_NEXT_OFFSET) { cmdrq->command = enum_value(MAV_CMD::DO_SET_ROI_WPNEXT_OFFSET); cmdrq->param1 = req->gimbal_device_id; cmdrq->param5 = req->pitch_offset; cmdrq->param6 = req->roll_offset; cmdrq->param7 = req->yaw_offset; - } - else if (req->mode == req->ROI_MODE_SYSID) { + } else if (req->mode == req->ROI_MODE_SYSID) { cmdrq->command = enum_value(MAV_CMD::DO_SET_ROI_SYSID); cmdrq->param1 = req->sysid; cmdrq->param2 = req->gimbal_device_id; - } - else if (req->mode == req->ROI_MODE_NONE) { + } else if (req->mode == req->ROI_MODE_NONE) { cmdrq->command = enum_value(MAV_CMD::DO_SET_ROI_NONE); cmdrq->param1 = req->gimbal_device_id; - } - else { + } else { res->success = false; res->result = 2; // MAV_RESULT_DENIED - Command is invalid (is supported but has invalid parameters). Retrying same command and parameters will not work. return; } - + // RCLCPP_DEBUG(get_logger(), "GimbalManagerSetRoi for gimbal id: %u ", req->gimbal_device_id); auto future = cmdClient->async_send_request(cmdrq); auto response = future.get(); @@ -635,7 +642,7 @@ class GimbalControlPlugin : public plugin::Plugin /** * @brief Set camera tracking mode and parameters * - * Message specifications: + * Message specifications: * https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT * https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_RECTANGLE * https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_STOP_TRACKING @@ -654,23 +661,20 @@ class GimbalControlPlugin : public plugin::Plugin cmdrq->param1 = req->x; cmdrq->param2 = req->y; cmdrq->param3 = req->radius; - } - else if (req->mode == req->CAMERA_TRACK_MODE_RECTANGLE) { + } else if (req->mode == req->CAMERA_TRACK_MODE_RECTANGLE) { cmdrq->command = enum_value(MAV_CMD::CAMERA_TRACK_RECTANGLE); cmdrq->param1 = req->top_left_x; cmdrq->param2 = req->top_left_y; cmdrq->param3 = req->bottom_right_x; cmdrq->param4 = req->bottom_right_y; - } - else if (req->mode == req->CAMERA_TRACK_MODE_STOP_TRACKING) { + } else if (req->mode == req->CAMERA_TRACK_MODE_STOP_TRACKING) { cmdrq->command = enum_value(MAV_CMD::CAMERA_STOP_TRACKING); - } - else { + } else { res->success = false; res->result = 2; // MAV_RESULT_DENIED - Command is invalid (is supported but has invalid parameters). Retrying same command and parameters will not work. return; } - + auto future = cmdClient->async_send_request(cmdrq); auto response = future.get(); res->success = response->success; diff --git a/mavros_extras/src/plugins/trajectory.cpp b/mavros_extras/src/plugins/trajectory.cpp index 51963c748..209dd1582 100644 --- a/mavros_extras/src/plugins/trajectory.cpp +++ b/mavros_extras/src/plugins/trajectory.cpp @@ -262,7 +262,7 @@ class TrajectoryPlugin : public plugin::Plugin auto fill_point_rep_waypoints = [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, const RosPoints & rp, - const size_t i) { + const size_t i) { const auto valid = req->point_valid[i]; auto valid_so_far = trajectory.valid_points; @@ -299,7 +299,7 @@ class TrajectoryPlugin : public plugin::Plugin mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER trajectory {}; auto fill_point_rep_bezier = [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER & t, const RosPoints & rp, - const size_t i) { + const size_t i) { const auto valid = req->point_valid[i]; auto valid_so_far = trajectory.valid_points; @@ -382,7 +382,7 @@ class TrajectoryPlugin : public plugin::Plugin auto fill_msg_point = [&](RosPoints & p, const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, - const size_t i) { + const size_t i) { fill_msg_position(p.position, t, i); fill_msg_velocity(p.velocity, t, i); fill_msg_acceleration(p.acceleration_or_force, t, i);