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

* add inverted-reverse param

(cherry picked from commit 2a5f3e4)

# Conflicts:
#	src/teleop_twist_joy.cpp

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

README.md

@@ -2,7 +2,7 @@ ros2/teleop_twist_joy
 ================
 
 # Overview
-The purpose of this package is to provide a generic facility for tele-operating Twist-based ROS2 robots with a standard joystick. 
+The purpose of this package is to provide a generic facility for tele-operating Twist-based ROS2 robots with a standard joystick.
 It converts joy messages to velocity commands.
 
 This node provides no rate limiting or autorepeat functionality. It is expected that you take advantage of the features built into [joy](https://index.ros.org/p/joy/github-ros-drivers-joystick_drivers/#foxy) for this.
@@ -25,7 +25,7 @@ The message type can be changed to `geometry_msgs/msg/TwistStamped` by the `publ
 
 - `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).
 
@@ -52,13 +52,13 @@ The message type can be changed to `geometry_msgs/msg/TwistStamped` by the `publ
   - `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)`
@@ -82,7 +82,7 @@ For most users building from source will not be required, execute `apt-get insta
 
 ## Run
 A launch file has been provided which has three arguments which can be changed in the terminal or via your own launch file.
-To configure the node to match your joystick a config file can be used. 
+To configure the node to match your joystick a config file can be used.
 There are several common ones provided in this package (atk3, ps3-holonomic, ps3, xbox, xd3), located here: https://github.com/ros2/teleop_twist_joy/tree/eloquent/config.
 
 PS3 is default, to run for another config (e.g. xbox) use this:

src/teleop_twist_joy.cpp

@@ -68,6 +68,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;
 
@@ -105,6 +107,8 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions& options) : Node("teleo
 
   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},
@@ -157,6 +161,8 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions& options) : Node("teleo
       "Teleop enable button %" PRId64 ".", pimpl_->enable_button);
   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)
@@ -362,10 +368,13 @@ void TeleopTwistJoy::Impl::fillCmdVelMsg(
   const std::string & which_map,
   geometry_msgs::msg::Twist * cmd_vel_msg)
 {
-  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");
 }

test/test_joy_twist.py

@@ -41,7 +41,7 @@ def setUp(self):
                                       automatically_declare_parameters_from_overrides=True)
         self.message_pump = launch_testing_ros.MessagePump(self.node, context=self.context)
         self.pub = self.node.create_publisher(sensor_msgs.msg.Joy, 'joy', 1)
-        if not hasattr(self, "cmd_vel_msg_type"):
+        if not hasattr(self, 'cmd_vel_msg_type'):
             self.cmd_vel_msg_type = geometry_msgs.msg.Twist
         self.sub = self.node.create_subscription(self.cmd_vel_msg_type,
                                                  'cmd_vel', self.callback, 1)