add inverted reverse param (#35) (#44)

* add inverted-reverse param

(cherry picked from commit 2a5f3e4)

Co-authored-by: Máté <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>

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.<axis>`
   - 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.<axis>`
   - 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

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<std::string, int64_t> axis_linear_map;
   std::map<std::string, std::map<std::string, double>> 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<std::string, int64_t> 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<std::string, int64_t>::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<rclcpp::PARAMETER_BOOL>();
+        } else if (parameter.get_name() == "inverted_reverse") {
+          this->pimpl_->inverted_reverse = parameter.get_value<rclcpp::PARAMETER_BOOL>();
         } else if (parameter.get_name() == "enable_button") {
           this->pimpl_->enable_button = parameter.get_value<rclcpp::PARAMETER_INTEGER>();
         } 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<geometry_msgs::msg::Twist>();
 
-  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");