Comp merge: ateam_joystick_control

ateam_joystick_control/config/common_params.yaml

@@ -1,3 +1,3 @@
 /joystick_control_node:
   ros__parameters:
-    dribbler_speed_step: 10.0
+    dribbler_speed_step: 0.01

ateam_joystick_control/config/xbox_joy_config.yaml

@@ -13,7 +13,7 @@
           axis: 3
           scale: 4.0
       kick: "axis 5 < -0.8"
-      chip: "button 5"
+      pivot: "button 5"
       dribbler:
         increment: "button 3"
         decrement: "button 2"

ateam_joystick_control/src/joystick_control_node.cpp

@@ -61,8 +61,8 @@ class JoystickControlNode : public rclcpp::Node
 
     kick_trigger_ = ParseTriggerFunction(declare_parameter<std::string>("mapping.kick",
         "axis 5 < -0.8"));
-    chip_trigger_ = ParseTriggerFunction(declare_parameter<std::string>("mapping.chip",
-        "button 5"));
+    pivot_trigger_ = ParseTriggerFunction(declare_parameter<std::string>("mapping.pivot",
+      "button 5"));
     dribbler_increment_trigger_ =
       ParseTriggerFunction(declare_parameter<std::string>("mapping.dribbler.increment",
         "button 3"));
@@ -77,16 +77,20 @@ class JoystickControlNode : public rclcpp::Node
         {"linear.x.scale", 1.0},
         {"linear.y.scale", 1.0},
         {"angular.z.scale", 1.0},
-        {"dribbler.max", 1000.0},
+        {"dribbler.max", 1.0},
         {"dribbler.min", 0.0}
       });
 
     // controls the size of steps when changing dribbler speed
-    declare_parameter<double>("dribbler_speed_step", 10.0);
+    declare_parameter<double>("dribbler_speed_step", 0.01);
 
     declare_parameter<double>("kick_speed", 5.0);
     declare_parameter<double>("chip_speed", 5.0);
 
+    // When true, joystick axes fill the global-frame pose (x, y, theta) and commands are sent as
+    // BCM_GLOBAL_POSITION instead of BCM_LOCAL_VELOCITY.
+    declare_parameter<bool>("use_global_position", false);
+
     CreatePublisher(declare_parameter<int>("robot_id", -1));
     parameter_callback_handle_ =
       add_on_set_parameters_callback(
@@ -105,7 +109,7 @@ class JoystickControlNode : public rclcpp::Node
   sensor_msgs::msg::Joy prev_joy_msg_;
 
   TriggerFunction kick_trigger_;
-  TriggerFunction chip_trigger_;
+  TriggerFunction pivot_trigger_;
   TriggerFunction dribbler_increment_trigger_;
   TriggerFunction dribbler_decrement_trigger_;
   TriggerFunction dribbler_spin_trigger_;
@@ -156,20 +160,34 @@ class JoystickControlNode : public rclcpp::Node
 
     ateam_msgs::msg::RobotMotionCommand command_message;
 
-    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.velocity.y = get_parameter("mapping.linear.y.scale").as_double() *
-      joy_message->axes[linear_y_axis_];
-    command_message.velocity.theta = get_parameter("mapping.angular.z.scale").as_double() *
-      joy_message->axes[angular_z_axis_];
+    if (pivot_trigger_(*joy_message)) {
+      command_message.body_control_mode = ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_PIVOT;
+      command_message.pivot_orbit_radius = kRobotRadius + kBallRadius;
+      command_message.pivot_inset_angle = M_PI / 2;
+
+      command_message.pivot_global_theta = M_PI * joy_message->axes[angular_z_axis_];
+    } else if (get_parameter("use_global_position").as_bool()) {
+      command_message.body_control_mode =
+        ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION;
+      command_message.pose.x = get_parameter("mapping.linear.x.scale").as_double() *
+        joy_message->axes[linear_x_axis_];
+      command_message.pose.y = get_parameter("mapping.linear.y.scale").as_double() *
+        joy_message->axes[linear_y_axis_];
+      command_message.pose.theta = get_parameter("mapping.angular.z.scale").as_double() *
+        joy_message->axes[angular_z_axis_];
+    } else {
+      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.velocity.y = get_parameter("mapping.linear.y.scale").as_double() *
+        joy_message->axes[linear_y_axis_];
+      command_message.velocity.theta = get_parameter("mapping.angular.z.scale").as_double() *
+        joy_message->axes[angular_z_axis_];
+    }
 
     if (kick_trigger_(*joy_message)) {
       command_message.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_KICK_TOUCH;
       command_message.kick_speed = get_parameter("kick_speed").as_double();
-    } else if (chip_trigger_(*joy_message)) {
-      command_message.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_CHIP_TOUCH;
-      command_message.kick_speed = get_parameter("chip_speed").as_double();
     } else {
       command_message.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_ARM;
     }
@@ -185,8 +203,10 @@ class JoystickControlNode : public rclcpp::Node
       dribbler_speed_, static_cast<float>(get_parameter("mapping.dribbler.min").as_double()),
       static_cast<float>(get_parameter("mapping.dribbler.max").as_double()));
     if (dribbler_spin_trigger_(*joy_message)) {
+      command_message.dribbler_mode = ateam_msgs::msg::RobotMotionCommand::DC_CURRENT;
       command_message.dribbler_setpoint = dribbler_speed_;
     } else {
+      command_message.dribbler_mode = ateam_msgs::msg::RobotMotionCommand::DC_DISABLE;
       command_message.dribbler_setpoint = 0.0;
     }