Add parameter to enable/disable requiring the enable button to be hel…

…d for motion (ros-teleop#21) * Add require_enable_button parameter Signed-off-by: Chris Lalancette <[email protected]> Co-authored-by: Chris Lalancette <[email protected]>
bdigney89 · Nov 30, 2020 · 9d86a01 · 9d86a01
1 parent affdc77
commit 9d86a01
Show file tree

Hide file tree

Showing 5 changed files with 67 additions and 3 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -79,6 +79,8 @@ if(BUILD_TESTING)
     test/turbo_enable_joy_launch_test.py
     test/only_turbo_joy_launch_test.py
     test/turbo_angular_enable_joy_launch_test.py
+
+    test/no_require_enable_joy_launch_test.py
   )
 
   find_package(launch_testing_ament_cmake REQUIRED)

diff --git a/README.md b/README.md
@@ -25,6 +25,9 @@ The package comes with the `teleop_node` that republishes `sensor_msgs/msg/Joy`
 - `axis_linear (int, default: 1)`
   - Joystick axis to use for linear movement control.
 
+- `require_enable_button (bool, default: true)`
+  - Whether to require the enable button for enabling movement.
+
 - `enable_button (int, default: 0)`
   - Joystick button to enable regular-speed movement.
 

diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp
@@ -56,6 +56,7 @@ struct TeleopTwistJoy::Impl
   rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub;
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub;
 
+  bool require_enable_button;
   int64_t enable_button;
   int64_t enable_turbo_button;
 
@@ -79,6 +80,8 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions& options) : Node("teleo
   pimpl_->joy_sub = this->create_subscription<sensor_msgs::msg::Joy>("joy", rclcpp::QoS(10),
     std::bind(&TeleopTwistJoy::Impl::joyCallback, this->pimpl_, std::placeholders::_1));
 
+  pimpl_->require_enable_button = this->declare_parameter("require_enable_button", true);
+
   pimpl_->enable_button = this->declare_parameter("enable_button", 5);
 
   pimpl_->enable_turbo_button = this->declare_parameter("enable_turbo_button", -1);
@@ -131,7 +134,8 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions& options) : Node("teleo
   this->declare_parameters("scale_angular_turbo", default_scale_angular_turbo_map);
   this->get_parameters("scale_angular_turbo", pimpl_->scale_angular_map["turbo"]);
 
-  ROS_INFO_NAMED("TeleopTwistJoy", "Teleop enable button %" PRId64 ".", pimpl_->enable_button);
+  ROS_INFO_COND_NAMED(pimpl_->require_enable_button, "TeleopTwistJoy", 
+      "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);
 
@@ -165,6 +169,7 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions& options) : Node("teleo
                                                  "scale_linear_turbo.x", "scale_linear_turbo.y", "scale_linear_turbo.z",
                                                  "scale_angular.yaw", "scale_angular.pitch", "scale_angular.roll",
                                                  "scale_angular_turbo.yaw", "scale_angular_turbo.pitch", "scale_angular_turbo.roll"};
+    static std::set<std::string> boolparams = {"require_enable_button"};
     auto result = rcl_interfaces::msg::SetParametersResult();
     result.successful = true;
 
@@ -191,11 +196,25 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions& options) : Node("teleo
           return result;
         }
       }
+      else if (boolparams.count(parameter.get_name()) == 1)
+      {
+        if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_BOOL)
+        {
+          result.reason = "Only boolean values can be set for '" + parameter.get_name() + "'.";
+          RCLCPP_WARN(this->get_logger(), result.reason.c_str());
+          result.successful = false;
+          return result;
+        }
+      }
     }
 
     // Loop to assign changed parameters to the member variables
     for (const auto & parameter : parameters)
     {
+      if (parameter.get_name() == "require_enable_button")
+      {
+        this->pimpl_->require_enable_button = parameter.get_value<rclcpp::PARAMETER_BOOL>();
+      }
       if (parameter.get_name() == "enable_button")
       {
         this->pimpl_->enable_button = parameter.get_value<rclcpp::PARAMETER_INTEGER>();
@@ -327,8 +346,9 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::msg::Joy::SharedPtr jo
   {
     sendCmdVelMsg(joy_msg, "turbo");
   }
-  else if (static_cast<int>(joy_msg->buttons.size()) > enable_button &&
-           joy_msg->buttons[enable_button])
+  else if (!require_enable_button ||
+	   (static_cast<int>(joy_msg->buttons.size()) > enable_button &&
+           joy_msg->buttons[enable_button]))
   {
     sendCmdVelMsg(joy_msg, "normal");
   }

diff --git a/test/no_enable_joy_launch_test.py b/test/no_enable_joy_launch_test.py
@@ -18,6 +18,7 @@ def generate_test_description():
             'scale_linear.x': 2.0,
             'scale_angular.yaw': 3.0,
             'enable_button': 0,
+            'require_enable_button': True
         }],
     )
 

diff --git a/test/no_require_enable_joy_launch_test.py b/test/no_require_enable_joy_launch_test.py
@@ -0,0 +1,38 @@
+import launch
+import launch_ros.actions
+import launch_testing
+
+import pytest
+
+import test_joy_twist
+
+
+@pytest.mark.rostest
+def generate_test_description():
+    teleop_node = launch_ros.actions.Node(
+        package='teleop_twist_joy',
+        executable='teleop_node',
+        parameters=[{
+            'axis_linear.x': 1,
+            'axis_angular.yaw': 0,
+            'scale_linear.x': 2.0,
+            'scale_angular.yaw': 3.0,
+            'enable_button': 0,
+            'require_enable_button': False,
+        }],
+    )
+
+    return launch.LaunchDescription([
+            teleop_node,
+            launch_testing.actions.ReadyToTest(),
+        ]), locals()
+
+
+class NoRequireEnableJoy(test_joy_twist.TestJoyTwist):
+
+    def setUp(self):
+        super().setUp()
+        self.joy_msg['axes'] = [0.3, 0.4]
+        self.joy_msg['buttons'] = [0]
+        self.expect_cmd_vel['linear']['x'] = 0.8
+        self.expect_cmd_vel['angular']['z'] = 0.9