diff --git a/README.md b/README.md index 8218482..1540a27 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ The package comes with the `teleop_node` that republishes `sensor_msgs/msg/Joy` - `enable_button (int, default: 0)` - Joystick button to enable regular-speed movement. - + - `enable_turbo_button (int, default: -1)` - Joystick button to enable high-speed movement (disabled when -1). @@ -51,21 +51,22 @@ The package comes with the `teleop_node` that republishes `sensor_msgs/msg/Joy` - `axis_angular.yaw (int, default: 2)` - `axis_angular.pitch (int, default: -1)` - `axis_angular.roll (int, default: -1)` - + - `scale_angular.` - Scale to apply to joystick angular axis. - `scale_angular.yaw (double, default: 0.5)` - `scale_angular.pitch (double, default: 0.0)` - `scale_angular.roll (double, default: 0.0)` - + - `scale_angular_turbo.` - Scale to apply to joystick angular axis for high-speed movement. - `scale_angular_turbo.yaw (double, default: 1.0)` - `scale_angular_turbo.pitch (double, default: 0.0)` - `scale_angular_turbo.roll (double, default: 0.0)` - - +- `inverted_reverse (bool, default: false)` + - Whether to invert turning left-right while reversing (useful for differential wheeled robots). + # Usage diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp index 2a17b9a..49d2c20 100644 --- a/src/teleop_twist_joy.cpp +++ b/src/teleop_twist_joy.cpp @@ -61,6 +61,8 @@ struct TeleopTwistJoy::Impl int64_t enable_button; int64_t enable_turbo_button; + bool inverted_reverse; + std::map axis_linear_map; std::map> scale_linear_map; @@ -89,6 +91,8 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions & options) pimpl_->enable_turbo_button = this->declare_parameter("enable_turbo_button", -1); + pimpl_->inverted_reverse = this->declare_parameter("inverted_reverse", false); + std::map default_linear_map{ {"x", 5L}, {"y", -1L}, @@ -143,6 +147,8 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions & options) ROS_INFO_COND_NAMED( pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", "Turbo on button %" PRId64 ".", pimpl_->enable_turbo_button); + ROS_INFO_COND_NAMED( + pimpl_->inverted_reverse, "TeleopTwistJoy", "%s", "Teleop enable inverted reverse."); for (std::map::iterator it = pimpl_->axis_linear_map.begin(); it != pimpl_->axis_linear_map.end(); ++it) @@ -180,6 +186,8 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions & options) for (const auto & parameter : parameters) { if (parameter.get_name() == "require_enable_button") { this->pimpl_->require_enable_button = parameter.get_value(); + } else if (parameter.get_name() == "inverted_reverse") { + this->pimpl_->inverted_reverse = parameter.get_value(); } else if (parameter.get_name() == "enable_button") { this->pimpl_->enable_button = parameter.get_value(); } else if (parameter.get_name() == "enable_turbo_button") { @@ -268,10 +276,13 @@ void TeleopTwistJoy::Impl::sendCmdVelMsg( // Initializes with zeros by default. auto cmd_vel_msg = std::make_unique(); - cmd_vel_msg->linear.x = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "x"); + double lin_x = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "x"); + double ang_z = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "yaw"); + + cmd_vel_msg->linear.x = lin_x; cmd_vel_msg->linear.y = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "y"); cmd_vel_msg->linear.z = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "z"); - cmd_vel_msg->angular.z = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "yaw"); + cmd_vel_msg->angular.z = (lin_x < 0.0 && inverted_reverse) ? -ang_z : ang_z; cmd_vel_msg->angular.y = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "pitch"); cmd_vel_msg->angular.x = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "roll");