diff --git a/ateam_msgs/msg/RobotMotionCommand.msg b/ateam_msgs/msg/RobotMotionCommand.msg index b47071a9..62d07aee 100644 --- a/ateam_msgs/msg/RobotMotionCommand.msg +++ b/ateam_msgs/msg/RobotMotionCommand.msg @@ -7,6 +7,10 @@ float64 limit_vel_angular # (rad/s) float64 limit_acc_linear # (m/s/s) float64 limit_acc_angular # (rad/s/s) +# pivot maneuver parameters (used when body_control_mode == BCM_PIVOT) +float64 pivot_orbit_radius # (m) +float64 pivot_heading_lag # (rad) + uint8 kick_request float32 kick_speed # (m/s) float32 dribbler_speed # (rpm) @@ -28,3 +32,4 @@ uint8 BCM_GLOBAL_VELOCITY = 2 uint8 BCM_LOCAL_VELOCITY = 3 uint8 BCM_GLOBAL_ACCEL = 4 uint8 BCM_LOCAL_ACCEL = 5 +uint8 BCM_PIVOT = 6 diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 256d8d01..8d762381 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -366,6 +366,18 @@ class RadioBridgeNode : public rclcpp::Node static_cast(command.acceleration.theta), }; break; + case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: + control_msg.body_control_mode = BCM_PIVOT; + control_msg.cmd.pivot = { + static_cast(command.pose.x), + static_cast(command.pose.y), + static_cast(command.pose.theta), + static_cast(command.limit_vel_angular), + static_cast(command.limit_acc_angular), + static_cast(command.pivot_orbit_radius), + static_cast(command.pivot_heading_lag) + }; + break; default: RCLCPP_WARN(get_logger(), "Unknown body control mode: %d", command.body_control_mode); control_msg.body_control_mode = BCM_OFF; diff --git a/radio/ateam_radio_msgs/CMakeLists.txt b/radio/ateam_radio_msgs/CMakeLists.txt index bbf2c618..e7703b97 100644 --- a/radio/ateam_radio_msgs/CMakeLists.txt +++ b/radio/ateam_radio_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ set(RADIO_STRUCTS_TO_GENERATE LocalVelocityCommand GlobalAccelerationCommand LocalAccelerationCommand + PivotCommand BasicTelemetry ErrorTelemetry ExtendedTelemetry diff --git a/radio/ateam_radio_msgs/software-communication b/radio/ateam_radio_msgs/software-communication index 5c79e614..f09f1c84 160000 --- a/radio/ateam_radio_msgs/software-communication +++ b/radio/ateam_radio_msgs/software-communication @@ -1 +1 @@ -Subproject commit 5c79e6148c4e9f7f023823adea44d122b2759b59 +Subproject commit f09f1c8498fc8c83d8644ccffb27ba185284af53