diff --git a/ateam_bringup/launch/autonomy.launch.xml b/ateam_bringup/launch/autonomy.launch.xml index 6f0a5f00f..d7a7ae962 100644 --- a/ateam_bringup/launch/autonomy.launch.xml +++ b/ateam_bringup/launch/autonomy.launch.xml @@ -1,5 +1,4 @@ - @@ -20,7 +19,6 @@ - diff --git a/ateam_joystick_control/src/joystick_control_node.cpp b/ateam_joystick_control/src/joystick_control_node.cpp index 02b58efda..68cda57af 100644 --- a/ateam_joystick_control/src/joystick_control_node.cpp +++ b/ateam_joystick_control/src/joystick_control_node.cpp @@ -156,11 +156,12 @@ class JoystickControlNode : public rclcpp::Node ateam_msgs::msg::RobotMotionCommand command_message; - command_message.twist.linear.x = get_parameter("mapping.linear.x.scale").as_double() * + command_message.body_control_mode = ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY; + command_message.velocity.x = get_parameter("mapping.linear.x.scale").as_double() * joy_message->axes[linear_x_axis_]; - command_message.twist.linear.y = get_parameter("mapping.linear.y.scale").as_double() * + command_message.velocity.y = get_parameter("mapping.linear.y.scale").as_double() * joy_message->axes[linear_y_axis_]; - command_message.twist.angular.z = get_parameter("mapping.angular.z.scale").as_double() * + command_message.velocity.theta = get_parameter("mapping.angular.z.scale").as_double() * joy_message->axes[angular_z_axis_]; if (kick_trigger_(*joy_message)) { diff --git a/ateam_joystick_control/test/launch_tests/joystick_control_node_test.py b/ateam_joystick_control/test/launch_tests/joystick_control_node_test.py index ca8b5f6a5..bbcf8b824 100644 --- a/ateam_joystick_control/test/launch_tests/joystick_control_node_test.py +++ b/ateam_joystick_control/test/launch_tests/joystick_control_node_test.py @@ -126,43 +126,15 @@ def setRobotId(self, robot_id): return TestJoystickControlNode.set_parameter_client \ .call(set_param_request) - def test_0_defaultRobotIdIsZero(self): - joy_msg = sensor_msgs.msg.Joy() - joy_msg.axes = [ - 1.0, - 1.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - joy_msg.buttons = [0] * 11 - - timeout = time.time() + 1 - while TestJoystickControlNode.received_msg_0 is None and \ - TestJoystickControlNode.received_msg_1 is None: - TestJoystickControlNode.pub.publish(joy_msg) - time.sleep(0.1) - if time.time() >= timeout: - break - - self.assertIsNotNone(TestJoystickControlNode.received_msg_0) - self.assertIsNone(TestJoystickControlNode.received_msg_1) - - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.x, 1.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.y, 1.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.z, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.x, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.y, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.z, 1.0) + def test_0_prepScenario(self): + # The control node defaults to ID -1, so we need to switch it to 0 first to make the rest + # of the test sequence work + set_param_response = self.setRobotId(0) + if not set_param_response.results[0].successful: + TestJoystickControlNode.node.get_logger() \ + .error(f'Setting parameter failed with reason: \ + {set_param_response.results[0].reason}') + self.assertTrue(set_param_response.results[0].successful) def test_1_shouldSendStopWhenJoystickSwitchesId(self): set_param_response = self.setRobotId(1) @@ -180,17 +152,11 @@ def test_1_shouldSendStopWhenJoystickSwitchesId(self): self.assertIsNotNone(TestJoystickControlNode.received_msg_0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.x, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.y, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.z, 0.0) + TestJoystickControlNode.received_msg_0.velocity.x, 0.0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.x, 0.0) + TestJoystickControlNode.received_msg_0.velocity.y, 0.0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.y, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.z, 0.0) + TestJoystickControlNode.received_msg_0.velocity.theta, 0.0) def test_2_afterIdChangeCommandsShouldGoToNewRobot(self): joy_msg = sensor_msgs.msg.Joy() @@ -215,17 +181,11 @@ def test_2_afterIdChangeCommandsShouldGoToNewRobot(self): self.assertIsNotNone(TestJoystickControlNode.received_msg_1) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.linear.x, -1.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.linear.y, -1.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.linear.z, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.angular.x, 0.0) + TestJoystickControlNode.received_msg_1.velocity.x, -1.0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.angular.y, 0.0) + TestJoystickControlNode.received_msg_1.velocity.y, -1.0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.angular.z, -1.0) + TestJoystickControlNode.received_msg_1.velocity.theta, -1.0) def test_3_invalidRobotIdsShouldBeRejected(self): self.assertFalse(self.setRobotId(-2).results[0].successful) diff --git a/ateam_kenobi/src/core/defense_area_enforcement.cpp b/ateam_kenobi/src/core/defense_area_enforcement.cpp index 6fc8f2e78..caed6e9ef 100644 --- a/ateam_kenobi/src/core/defense_area_enforcement.cpp +++ b/ateam_kenobi/src/core/defense_area_enforcement.cpp @@ -43,8 +43,8 @@ void EnforceDefenseAreaKeepout( } auto & command = *maybe_command; if (WouldVelocityCauseCollision(world, robot_id, command)) { - command.twist.linear.x = 0.0; - command.twist.linear.y = 0.0; + command.velocity.x = 0.0; + command.velocity.y = 0.0; } } } @@ -72,11 +72,13 @@ bool WouldVelocityCauseCollision( const double delta_t = 0.01; - ateam_geometry::Vector velocity{motion_command.twist.linear.x, - motion_command.twist.linear.y}; - if (motion_command.twist_frame == ateam_msgs::msg::RobotMotionCommand::FRAME_BODY) { - velocity = ateam_kenobi::motion::LocalToWorldFrame(velocity, world.our_robots[robot_id]); + if (motion_command.body_control_mode != ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY) { + std::cerr << + "WARNING: Non-local velocity motion commands not yet supported by defense area enforcement. " + "Skipping check.\n"; + return false; } + ateam_geometry::Vector velocity{motion_command.velocity.x, motion_command.velocity.y}; const ateam_geometry::Vector displacement = velocity * delta_t; diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index ebbbe965d..7447f8282 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -69,7 +69,6 @@ class KenobiNode : public rclcpp::Node overlays_(""), motion_executor_(get_logger().get_child("motion")) { - declare_parameter("use_world_velocities", false); declare_parameter("use_emulated_ballsense", false); overlay_publisher_ = create_publisher( @@ -223,8 +222,6 @@ class KenobiNode : public rclcpp::Node }); const auto motion_commands = motion_executor_.RunFrame(motion_intents, overlays_, world); - const auto use_world_vels = get_parameter("use_world_velocities").as_bool(); - std::array, 16> ros_commands; for(auto id = 0ul; id < commands.size(); ++id) { auto & maybe_cmd = commands[id]; @@ -232,21 +229,17 @@ class KenobiNode : public rclcpp::Node if (!maybe_cmd || !maybe_motion_cmd) { ros_commands[id] = std::nullopt; } else { - const auto & robot = world.our_robots[id]; const auto & cmd = maybe_cmd.value(); const auto & motion_cmd = maybe_motion_cmd.value(); - const auto linear_vel = use_world_vels ? - motion::LocalToWorldFrame(motion_cmd.linear, robot) : motion_cmd.linear; + const auto linear_vel = motion_cmd.linear; auto & ros_cmd = ros_commands[id].emplace(); ros_cmd.dribbler_speed = cmd.dribbler_speed; ros_cmd.kick_request = static_cast(cmd.kick); ros_cmd.kick_speed = cmd.kick_speed; - ros_cmd.twist.linear.x = linear_vel.x(); - ros_cmd.twist.linear.y = linear_vel.y(); - ros_cmd.twist.angular.z = motion_cmd.angular; - ros_cmd.twist_frame = - use_world_vels ? ateam_msgs::msg::RobotMotionCommand::FRAME_WORLD : - ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; + ros_cmd.body_control_mode = ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY; + ros_cmd.velocity.x = linear_vel.x(); + ros_cmd.velocity.y = linear_vel.y(); + ros_cmd.velocity.theta = motion_cmd.angular; } } diff --git a/ateam_msgs/CMakeLists.txt b/ateam_msgs/CMakeLists.txt index 139cc0d92..be02fcf5c 100755 --- a/ateam_msgs/CMakeLists.txt +++ b/ateam_msgs/CMakeLists.txt @@ -16,7 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/OverlayArray.msg msg/RefereeInfo.msg msg/RobotMotionCommand.msg - msg/TeamClientConnectionStatus.msg + msg/Twist2D.msg msg/GameStateBall.msg msg/GameStateRobot.msg @@ -37,13 +37,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/PlayInfo.msg srv/GetFirmwareParameter.srv - srv/ReconnectTeamClient.srv - srv/SetDesiredKeeper.srv srv/SetFirmwareParameter.srv srv/SetOverridePlay.srv srv/SetPlayEnabled.srv - srv/SetTeamAdvantageChoice.srv - srv/SubstituteBot.srv srv/SetIgnoreFieldSide.srv srv/SendSimulatorControlPacket.srv diff --git a/ateam_msgs/msg/RobotMotionCommand.msg b/ateam_msgs/msg/RobotMotionCommand.msg index ce22894d1..b47071a9f 100644 --- a/ateam_msgs/msg/RobotMotionCommand.msg +++ b/ateam_msgs/msg/RobotMotionCommand.msg @@ -1,11 +1,15 @@ -# Global velocity commands -# XY in translation (m/s) -# Z in rotation (rad/s) -geometry_msgs/Twist twist -uint8 twist_frame 0 +uint8 body_control_mode +Twist2D pose # (m, rad) +Twist2D velocity # (m/s, rad/s) +Twist2D acceleration # (m/s/s, rad/s/s) +float64 limit_vel_linear # (m/s) +float64 limit_vel_angular # (rad/s) +float64 limit_acc_linear # (m/s/s) +float64 limit_acc_angular # (rad/s/s) + uint8 kick_request -float32 kick_speed # (m/s) -float32 dribbler_speed # (rpm) +float32 kick_speed # (m/s) +float32 dribbler_speed # (rpm) # kick options uint8 KR_ARM = 0 @@ -17,6 +21,10 @@ uint8 KR_CHIP_NOW = 5 uint8 KR_CHIP_TOUCH = 6 uint8 KR_CHIP_CAPTURED = 7 -# frame options -uint8 FRAME_WORLD = 0 -uint8 FRAME_BODY = 1 +# body control modes +uint8 BCM_OFF = 0 +uint8 BCM_GLOBAL_POSITION = 1 +uint8 BCM_GLOBAL_VELOCITY = 2 +uint8 BCM_LOCAL_VELOCITY = 3 +uint8 BCM_GLOBAL_ACCEL = 4 +uint8 BCM_LOCAL_ACCEL = 5 diff --git a/ateam_msgs/msg/Twist2D.msg b/ateam_msgs/msg/Twist2D.msg new file mode 100644 index 000000000..3acb85874 --- /dev/null +++ b/ateam_msgs/msg/Twist2D.msg @@ -0,0 +1,3 @@ +float64 x +float64 y +float64 theta diff --git a/ateam_msgs/srv/ReconnectTeamClient.srv b/ateam_msgs/srv/ReconnectTeamClient.srv deleted file mode 100644 index 251ec6234..000000000 --- a/ateam_msgs/srv/ReconnectTeamClient.srv +++ /dev/null @@ -1,9 +0,0 @@ -# Request - -# Address of the server to connect to. If blank, will reconnect to the last used address. -string server_address "" - ---- -# Response - -bool success diff --git a/ateam_msgs/srv/SetDesiredKeeper.srv b/ateam_msgs/srv/SetDesiredKeeper.srv deleted file mode 100644 index 8a54d577b..000000000 --- a/ateam_msgs/srv/SetDesiredKeeper.srv +++ /dev/null @@ -1,6 +0,0 @@ -# Request -int32 desired_keeper ---- -# Response -bool success -string reason diff --git a/ateam_msgs/srv/SetTeamAdvantageChoice.srv b/ateam_msgs/srv/SetTeamAdvantageChoice.srv deleted file mode 100644 index 6cb00d6c4..000000000 --- a/ateam_msgs/srv/SetTeamAdvantageChoice.srv +++ /dev/null @@ -1,10 +0,0 @@ -# Request -uint8 choice - -uint8 STOP=0 -uint8 CONTINUE=1 - ---- -# Response -bool success -string reason diff --git a/ateam_msgs/srv/SubstituteBot.srv b/ateam_msgs/srv/SubstituteBot.srv deleted file mode 100644 index 7b785ef4e..000000000 --- a/ateam_msgs/srv/SubstituteBot.srv +++ /dev/null @@ -1,5 +0,0 @@ -# Request ---- -# Response -bool success -string reason diff --git a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp index 1304cadf7..2f7104dfc 100644 --- a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp @@ -346,8 +346,8 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node command_speed_ = speed; - command.twist.angular.z = command_speed_; - command.twist.linear.y = -1 * command_speed_ * options_.radius; + command.velocity.theta = command_speed_; + command.velocity.y = -1 * command_speed_ * options_.radius; command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; @@ -376,8 +376,8 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node { timer_.reset(); ateam_msgs::msg::RobotMotionCommand command; - command.twist.linear.x = 0.0; - command.twist.linear.y = 0.0; + command.velocity.x = 0.0; + command.velocity.y = 0.0; command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; command_pub_->publish(command); std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp index 3d4a65ce6..a177cd5b1 100644 --- a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp @@ -350,9 +350,9 @@ class VelocityBenchmarkNode : public rclcpp::Node command_speed_ = speed; if(options_.axis == Axis::X) { - command.twist.linear.x = speed; + command.velocity.x = speed; } else { - command.twist.linear.y = speed; + command.velocity.y = speed; } command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; @@ -390,8 +390,8 @@ class VelocityBenchmarkNode : public rclcpp::Node { timer_.reset(); ateam_msgs::msg::RobotMotionCommand command; - command.twist.linear.x = 0.0; - command.twist.linear.y = 0.0; + command.velocity.x = 0.0; + command.velocity.y = 0.0; command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; command_pub_->publish(command); std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index a6680322e..1e6c55762 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -169,6 +169,19 @@ class RadioBridgeNode : public rclcpp::Node } } + void ReplaceNanWithZero(float & val) { + if (std::isnan(val)) { + RCLCPP_WARN(get_logger(), "Radio bridge is replacing NaNs!"); + val = 0.0f; + } + } + + void ReplaceNanWithZero(ateam_msgs::msg::Twist2D & twist) { + ReplaceNanWithZero(twist.x); + ReplaceNanWithZero(twist.y); + ReplaceNanWithZero(twist.theta); + } + void MotionCommandCallback( const ateam_msgs::msg::RobotMotionCommand::SharedPtr command_msg, int robot_id) @@ -176,9 +189,15 @@ class RadioBridgeNode : public rclcpp::Node const std::lock_guard lock(mutex_); motion_commands_[robot_id] = *command_msg; auto & command = motion_commands_[robot_id]; - ReplaceNanWithZero(command.twist.linear.x); - ReplaceNanWithZero(command.twist.linear.y); - ReplaceNanWithZero(command.twist.angular.z); + ReplaceNanWithZero(command.pose); + ReplaceNanWithZero(command.velocity); + ReplaceNanWithZero(command.acceleration); + ReplaceNanWithZero(command.limit_vel_linear); + ReplaceNanWithZero(command.limit_vel_angular); + ReplaceNanWithZero(command.limit_acc_linear); + ReplaceNanWithZero(command.limit_acc_angular); + ReplaceNanWithZero(command.kick_speed); + ReplaceNanWithZero(command.dribbler_speed); motion_command_timestamps_[robot_id] = std::chrono::steady_clock::now(); } @@ -272,19 +291,13 @@ class RadioBridgeNode : public rclcpp::Node control_msg.vision_position_update[0] = 0.0f; control_msg.vision_position_update[1] = 0.0f; control_msg.vision_position_update[2] = 0.0f; - control_msg.body_control_mode = BCM_LOCAL_VELOCITY; control_msg.kick_request = static_cast(motion_commands_[id].kick_request); control_msg.play_song = 0; control_msg.reserved2[0] = 0; control_msg.kick_vel = motion_commands_[id].kick_speed; control_msg.dribbler_speed = motion_commands_[id].dribbler_speed; - control_msg.cmd.local_vel = { - static_cast(motion_commands_[id].twist.linear.x), - static_cast(motion_commands_[id].twist.linear.y), - static_cast(motion_commands_[id].twist.angular.z), - 0.0, // TODO(barulicm): max_linear_acc - 0.0 // TODO(barulicm): max_angular_acc - }; + FillBodyControl(control_msg, motion_commands_[id]); + const auto control_packet = CreatePacket(CC_CONTROL, control_msg); connections_[id]->send( reinterpret_cast(&control_packet), @@ -292,6 +305,66 @@ class RadioBridgeNode : public rclcpp::Node } } + void FillBodyControl(BasicControl & control_msg, const ateam_msgs::msg::RobotMotionCommand & command) { + switch(command.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: + control_msg.body_control_mode = BCM_OFF; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + control_msg.body_control_mode = BCM_GLOBAL_POSITION; + control_msg.cmd.global_pos = { + static_cast(command.pose.x), + static_cast(command.pose.y), + static_cast(command.pose.theta), + static_cast(command.limit_vel_linear), + static_cast(command.limit_vel_angular), + static_cast(command.limit_acc_linear), + static_cast(command.limit_acc_angular) + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: + control_msg.body_control_mode = BCM_GLOBAL_VELOCITY; + control_msg.cmd.global_vel = { + static_cast(command.velocity.x), + static_cast(command.velocity.y), + static_cast(command.velocity.theta), + static_cast(command.limit_vel_linear), + static_cast(command.limit_vel_angular) + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: + control_msg.body_control_mode = BCM_LOCAL_VELOCITY; + control_msg.cmd.local_vel = { + static_cast(command.velocity.x), + static_cast(command.velocity.y), + static_cast(command.velocity.theta), + static_cast(command.limit_vel_linear), + static_cast(command.limit_vel_angular) + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_ACCEL: + control_msg.body_control_mode = BCM_GLOBAL_ACCEL; + control_msg.cmd.global_acc = { + static_cast(command.acceleration.x), + static_cast(command.acceleration.y), + static_cast(command.acceleration.theta), + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_ACCEL: + control_msg.body_control_mode = BCM_LOCAL_ACCEL; + control_msg.cmd.local_acc = { + static_cast(command.acceleration.x), + static_cast(command.acceleration.y), + static_cast(command.acceleration.theta), + }; + break; + default: + RCLCPP_WARN(get_logger(), "Unknown body control mode: %d", command.body_control_mode); + control_msg.body_control_mode = BCM_OFF; + break; + } + } + void DiscoveryMessageCallback( const std::string & sender_address, const uint16_t sender_port, uint8_t * udp_packet_data, size_t udp_packet_size) diff --git a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py index e84ea8997..bb50bd4cf 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py +++ b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py @@ -74,7 +74,8 @@ def test_commands(self): self.assertTrue(self.robot.isConnected()) cmd_msg = ateam_msgs.msg.RobotMotionCommand() - cmd_msg.twist.linear.x = 2.0 + cmd_msg.body_control_mode = ateam_msgs.msg.RobotMotionCommand.BCM_LOCAL_VELOCITY + cmd_msg.velocity.x = 2.0 timeout = time.time() + 1 while True: diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp index 2a87e1fc6..3d9b1c8c7 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp @@ -80,9 +80,9 @@ RobotControl fromMsg( RobotMoveCommand * robot_move_command = proto_robot_command->mutable_move_command(); MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - local_velocity_command->set_forward(ReplaceNanWithZero(ros_msg.twist.linear.x, logger)); - local_velocity_command->set_left(ReplaceNanWithZero(ros_msg.twist.linear.y, logger)); - local_velocity_command->set_angular(ReplaceNanWithZero(ros_msg.twist.angular.z, logger)); + local_velocity_command->set_forward(ReplaceNanWithZero(ros_msg.velocity.x, logger)); + local_velocity_command->set_left(ReplaceNanWithZero(ros_msg.velocity.y, logger)); + local_velocity_command->set_angular(ReplaceNanWithZero(ros_msg.velocity.theta, logger)); return robots_control; } diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp index 02b125477..989bae0e5 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp @@ -208,8 +208,9 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node ateam_msgs::msg::RobotMotionCommand msg; msg.dribbler_speed = 0.0; msg.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; - msg.twist.linear.x = 0.0; - msg.twist.angular.z = 0.0; + msg.velocity.x = 0.0; + msg.velocity.y = 0.0; + msg.velocity.theta = 0.0; send_command(msg, id); } diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp index 779b5d8ee..c00c4a579 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -214,9 +214,12 @@ class GameStateTracker : public rclcpp::Node { void RobotCommandCallback(const ateam_msgs::msg::RobotMotionCommand::SharedPtr msg, int id) { - world_.our_robots[id].prev_command_vel = ateam_geometry::Vector(msg->twist.linear.x, - msg->twist.linear.y); - world_.our_robots[id].prev_command_omega = msg->twist.angular.z; + // TODO(barulicm): This assumes we only use the local velocity mode + // This is fine for now because by the time we support other modes in Kenobi, + // we shouldn't depend on these fields anyway + world_.our_robots[id].prev_command_vel = ateam_geometry::Vector(msg->velocity.x, + msg->velocity.y); + world_.our_robots[id].prev_command_omega = msg->velocity.theta; } void UpdateRefInfo()