From 02a454ff3f4506f2883a527116c4bc85f490ed17 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 8 Jul 2026 23:59:07 +0900 Subject: [PATCH 1/4] Imports joystick control changes from comp branch --- .../config/common_params.yaml | 2 +- .../config/xbox_joy_config.yaml | 2 +- .../src/joystick_control_node.cpp | 47 +++++++++++++++---- 3 files changed, 39 insertions(+), 12 deletions(-) diff --git a/ateam_joystick_control/config/common_params.yaml b/ateam_joystick_control/config/common_params.yaml index 267daf43f..6a4898c32 100644 --- a/ateam_joystick_control/config/common_params.yaml +++ b/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 diff --git a/ateam_joystick_control/config/xbox_joy_config.yaml b/ateam_joystick_control/config/xbox_joy_config.yaml index 4a531938b..c96188cb5 100644 --- a/ateam_joystick_control/config/xbox_joy_config.yaml +++ b/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" diff --git a/ateam_joystick_control/src/joystick_control_node.cpp b/ateam_joystick_control/src/joystick_control_node.cpp index 33a390bd7..045dee3cb 100644 --- a/ateam_joystick_control/src/joystick_control_node.cpp +++ b/ateam_joystick_control/src/joystick_control_node.cpp @@ -62,7 +62,9 @@ class JoystickControlNode : public rclcpp::Node kick_trigger_ = ParseTriggerFunction(declare_parameter("mapping.kick", "axis 5 < -0.8")); chip_trigger_ = ParseTriggerFunction(declare_parameter("mapping.chip", - "button 5")); + "")); + pivot_trigger_ = ParseTriggerFunction(declare_parameter("mappign.pivot", + "button 5")); dribbler_increment_trigger_ = ParseTriggerFunction(declare_parameter("mapping.dribbler.increment", "button 3")); @@ -77,16 +79,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("dribbler_speed_step", 10.0); + declare_parameter("dribbler_speed_step", 0.01); declare_parameter("kick_speed", 5.0); declare_parameter("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("use_global_position", false); + CreatePublisher(declare_parameter("robot_id", -1)); parameter_callback_handle_ = add_on_set_parameters_callback( @@ -106,6 +112,7 @@ class JoystickControlNode : public rclcpp::Node TriggerFunction kick_trigger_; TriggerFunction chip_trigger_; + TriggerFunction pivot_trigger_; TriggerFunction dribbler_increment_trigger_; TriggerFunction dribbler_decrement_trigger_; TriggerFunction dribbler_spin_trigger_; @@ -156,13 +163,31 @@ 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_]; // This is truly cursed + } 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; @@ -185,8 +210,10 @@ class JoystickControlNode : public rclcpp::Node dribbler_speed_, static_cast(get_parameter("mapping.dribbler.min").as_double()), static_cast(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; } From 307f3b4954d03d440b5fd36fa4ef6693cca8dcc0 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Thu, 9 Jul 2026 00:00:05 +0900 Subject: [PATCH 2/4] Removes unused chip trigger --- ateam_joystick_control/src/joystick_control_node.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/ateam_joystick_control/src/joystick_control_node.cpp b/ateam_joystick_control/src/joystick_control_node.cpp index 045dee3cb..7dbf990d4 100644 --- a/ateam_joystick_control/src/joystick_control_node.cpp +++ b/ateam_joystick_control/src/joystick_control_node.cpp @@ -61,8 +61,6 @@ class JoystickControlNode : public rclcpp::Node kick_trigger_ = ParseTriggerFunction(declare_parameter("mapping.kick", "axis 5 < -0.8")); - chip_trigger_ = ParseTriggerFunction(declare_parameter("mapping.chip", - "")); pivot_trigger_ = ParseTriggerFunction(declare_parameter("mappign.pivot", "button 5")); dribbler_increment_trigger_ = @@ -111,7 +109,6 @@ 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_; @@ -192,9 +189,6 @@ class JoystickControlNode : public rclcpp::Node 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; } From 1047c17537c90a522b794dbb60337fe035b60db3 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Thu, 9 Jul 2026 00:02:57 +0900 Subject: [PATCH 3/4] Fixes linting failures --- .../src/joystick_control_node.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/ateam_joystick_control/src/joystick_control_node.cpp b/ateam_joystick_control/src/joystick_control_node.cpp index 7dbf990d4..b4d2d7151 100644 --- a/ateam_joystick_control/src/joystick_control_node.cpp +++ b/ateam_joystick_control/src/joystick_control_node.cpp @@ -163,10 +163,9 @@ class JoystickControlNode : public rclcpp::Node 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_inset_angle = M_PI / 2; - command_message.pivot_global_theta = M_PI * - joy_message->axes[angular_z_axis_]; // This is truly cursed + 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; @@ -178,12 +177,12 @@ class JoystickControlNode : public rclcpp::Node 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_]; + 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)) { From 42a4bed3cf6801d7359500d8b6d28ab79d8afd77 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Thu, 9 Jul 2026 00:06:55 +0900 Subject: [PATCH 4/4] Fixes param name typo --- ateam_joystick_control/src/joystick_control_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ateam_joystick_control/src/joystick_control_node.cpp b/ateam_joystick_control/src/joystick_control_node.cpp index b4d2d7151..73fa38cf8 100644 --- a/ateam_joystick_control/src/joystick_control_node.cpp +++ b/ateam_joystick_control/src/joystick_control_node.cpp @@ -61,7 +61,7 @@ class JoystickControlNode : public rclcpp::Node kick_trigger_ = ParseTriggerFunction(declare_parameter("mapping.kick", "axis 5 < -0.8")); - pivot_trigger_ = ParseTriggerFunction(declare_parameter("mappign.pivot", + pivot_trigger_ = ParseTriggerFunction(declare_parameter("mapping.pivot", "button 5")); dribbler_increment_trigger_ = ParseTriggerFunction(declare_parameter("mapping.dribbler.increment",