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..73fa38cf8 100644 --- a/ateam_joystick_control/src/joystick_control_node.cpp +++ b/ateam_joystick_control/src/joystick_control_node.cpp @@ -61,8 +61,8 @@ 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("mapping.pivot", + "button 5")); dribbler_increment_trigger_ = ParseTriggerFunction(declare_parameter("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("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( @@ -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(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; }