Adds control modes to RobotMotionCommand

ateam_bringup/launch/autonomy.launch.xml

@@ -1,5 +1,4 @@
 <launch>
-  <arg name="use_world_velocities" default="false"/>
   <arg name="use_emulated_ballsense" default="false"/>
   <arg name="team_name" default="A-Team"/>
   <arg name="kenobi_debug" default="false"/>
@@ -20,7 +19,6 @@
   </node>
   <let name="kenobi_prefix" value="$(eval '\'xterm -fg white -bg black -e gdb -ex run --args\' if (\'$(var kenobi_debug)\'.lower() == \'true\') else \'\'')"/>
   <node name="kenobi_node" pkg="ateam_kenobi" exec="ateam_kenobi_node" respawn="True" launch-prefix="$(var kenobi_prefix)">
-    <param name="use_world_velocities" value="$(var use_world_velocities)"/>
     <param name="use_emulated_ballsense" value="$(var use_emulated_ballsense)"/>
     <param name="playbook" value="$(var kenobi_playbook)"/>
   </node>

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)) {

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)

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;
 

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<bool>("use_world_velocities", false);
     declare_parameter<bool>("use_emulated_ballsense", false);
 
     overlay_publisher_ = create_publisher<ateam_msgs::msg::OverlayArray>(
@@ -223,30 +222,24 @@ 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<std::optional<ateam_msgs::msg::RobotMotionCommand>, 16> ros_commands;
     for(auto id = 0ul; id < commands.size(); ++id) {
       auto & maybe_cmd = commands[id];
       auto & maybe_motion_cmd = motion_commands[id];
       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<uint8_t>(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;
       }
     }
 

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
 

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

ateam_msgs/msg/Twist2D.msg

@@ -0,0 +1,3 @@
+float64 x
+float64 y
+float64 theta

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));

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));