ROS 2 add E-stop (#15)

* add e-stop handle * update README * update panther compose * remove old comment * Update README.md Co-authored-by: Krzysztof Wojciechowski <[email protected]> * Update joy2twist/src/joy2twist_node.cpp Co-authored-by: Krzysztof Wojciechowski <[email protected]> * Update joy2twist/src/joy2twist_node.cpp Co-authored-by: Krzysztof Wojciechowski <[email protected]> * Update joy2twist/include/joy2twist/joy2twist_node.hpp Co-authored-by: Krzysztof Wojciechowski <[email protected]> * Update joy2twist/src/joy2twist_node.cpp Co-authored-by: Krzysztof Wojciechowski <[email protected]> * review fixes * fix comment --------- Co-authored-by: Krzysztof Wojciechowski <[email protected]>
husarion · Dec 5, 2023 · 4c4b1cd · 4c4b1cd
1 parent bdc8b1d
commit 4c4b1cd
Show file tree

Hide file tree

Showing 8 changed files with 156 additions and 33 deletions.
diff --git a/.clang-format b/.clang-format
@@ -14,4 +14,8 @@ ConstructorInitializerIndentWidth: 0
 ContinuationIndentWidth: 2
 DerivePointerAlignment: false
 PointerAlignment: Middle
-ReflowComments: false
+ReflowComments: true
+IncludeBlocks: Preserve
+AlignOperands: true
+PenaltyBreakAssignment: 21
+PenaltyBreakBeforeFirstCallParameter: 1
diff --git a/README.md b/README.md
@@ -17,17 +17,33 @@ See `ls /dev/input | grep js` and find your joy number. If it differs, apply cha
 
 ## Button mapping
 
-|  Button  |      Function      |
-|:--------:|:------------------:|
-|   `LB`   |   enable driving   |
-|   `RB`   | slow driving mode  |
-|   `RT`   |  fast driving mode |
+| Button |     Function      |
+| :----: | :---------------: |
+|  `LB`  |  enable driving   |
+|  `RB`  | slow driving mode |
+|  `RT`  | fast driving mode |
 
 If neither `RB` nor `RT` is pressed, the robot operates in *regular* driving mode.
 
 To drive robot use sticks.
 By default, linear `X` and `Y` are held by the left stick. Angular `Z` is controlled with the right stick.
 
+### Emergency stop
+
+| Button |      Function       |
+| :----: | :-----------------: |
+|  `A`   |    Reset E-stop     |
+|  `B`   |   Trigger E-stop    |
+|  `LT`  | Enable E-stop reset |
+
+> [!NOTE] 
+> Handle of robot's emergency stop is available only when `~e_stop/present` parameter is set true. This functionality will work with any robot configured as follows:
+> - publishes robot's E-stop state uisng ROS topic of type `std_msgs/Bool`.
+> - allows resetting robot's E-stop using ROS service of type `std_srvs/Trigger`.
+> - allows triggering robot's E-stop using ROS service of type `std_srvs/Trigger`.
+> 
+> Topic and services names can be configured using ROS parameters, see [Parameters](#parameters) for more info.
+
 ---
 ## ROS node API
 
@@ -56,14 +72,21 @@ The robot can be operated at 3 scales of speed depending on pressed buttons. It'
 - `regular` *(float, default: 0.5)*
 - `slow`    *(float, default: 0.2)*
 
+The node can be configured using parameters described below to work with robots equipped with an E-stop interface. An example configuration for a robot with an E-stop interface can be found in [panther config file](./joy2twist/config/joy2twist_panther.yaml).
+
+- `~e_stop/present`         *(bool, default: false)*
+- `~e_stop/topic`           *(string, default: e_stop)*
+- `~e_stop/reset_srv`       *(string, default: e_stop_reset)*
+- `~e_stop/trigger_srv`     *(string, default: e_stop_trigger)*
+
 ## Docker image
 
 [![Build/Publish Docker Image](https://github.com/husarion/joy2twist/actions/workflows/build-docker-image.yaml/badge.svg)](https://github.com/husarion/joy2twist/actions/workflows/build-docker-image.yaml)
 
-| ROS2 distro | Supported architectures |
-| - | - |
-| `galactic` | `linux/amd64`, `linux/arm64` |
-| `humble` | `linux/amd64`, `linux/arm64` |
+| ROS2 distro |   Supported architectures    |
+| :---------: | :--------------------------: |
+| `galactic`  | `linux/amd64`, `linux/arm64` |
+|  `humble`   | `linux/amd64`, `linux/arm64` |
 
 Available on [Docker Hub](https://hub.docker.com/r/husarion/joy2twist/tags)
 
@@ -123,4 +146,4 @@ To run the `joy2twist` container execute the following command on your PC in the
 
 ```
 docker compose -f compose.pc.yaml up
-```
+```
diff --git a/demo/single_robot/compose.panther.yaml b/demo/single_robot/compose.panther.yaml
@@ -1,30 +1,18 @@
 services:
 
   joy2twist:
-    image: husarion/joy2twist:galactic
+    image: husarion/joy2twist:humble-nightly
     restart: unless-stopped
     tty: true
     network_mode: host
     ipc: host
     devices:
       - /dev/input/js0
-    environment:
-      - ROS_MASTER_URI=http://10.15.20.2:11311
-    volumes: 
-      - ../joy2twist/config/joy2twist_panther.yaml:/joy2twist.yaml
-    command: >
-      ros2 launch
-      joy2twist gamepad_controller.launch.py
-      joy2twist_params_file:=/joy2twist.yaml
-
-  bridge:
-    image: husarion/ros:galactic-ros1-bridge
-    restart: unless-stopped
-    network_mode: host
-    ipc: host
     environment:
       - ROS_IP=10.15.20.2
       - ROS_MASTER_URI=http://10.15.20.2:11311
     command: >
-      ros2 run
-      ros1_bridge dynamic_bridge
+      bash -c
+      "ros2 launch 
+      joy2twist gamepad_controller.launch.py
+      joy2twist_params_file:=$(ros2 pkg prefix joy2twist)/share/joy2twist/config/joy2twist_panther.yaml"
diff --git a/joy2twist/CMakeLists.txt b/joy2twist/CMakeLists.txt
@@ -17,6 +17,8 @@ find_package(ament_index_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(joy_linux REQUIRED)
 include_directories(include)
 
@@ -32,6 +34,8 @@ ament_target_dependencies(joy2twist
   rclcpp
   geometry_msgs  
   sensor_msgs
+  std_msgs
+  std_srvs
   joy_linux
 )
 

diff --git a/joy2twist/config/joy2twist_panther.yaml b/joy2twist/config/joy2twist_panther.yaml
@@ -10,6 +10,12 @@
       regular: 0.5
       slow: 0.25
 
+    e_stop:
+      present: true
+      topic: /panther/hardware/e_stop
+      reset_srv: /panther/hardware/e_stop_reset
+      trigger_srv: /panther/hardware/e_stop_trigger
+
     # This button mapping should be adjusted to the specific controller
     # The following map is suited for Logitech F710
     button_index_map:
@@ -20,3 +26,6 @@
       dead_man_switch: 4 # LB
       fast_mode: 7 # RT
       slow_mode: 5 # RB
+      e_stop_reset: 1 # A
+      e_stop_trigger: 2 # B
+      enable_e_stop_reset: 6 # LT
diff --git a/joy2twist/include/joy2twist/joy2twist_node.hpp b/joy2twist/include/joy2twist/joy2twist_node.hpp
@@ -9,11 +9,15 @@
 
 #include <geometry_msgs/msg/twist.hpp>
 #include <sensor_msgs/msg/joy.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 namespace joy2twist
 {
 using MsgJoy = sensor_msgs::msg::Joy;
 using MsgTwist = geometry_msgs::msg::Twist;
+using MsgBool = std_msgs::msg::Bool;
+using SrvTrigger = std_srvs::srv::Trigger;
 
 struct ButtonIndex
 {
@@ -24,6 +28,10 @@ struct ButtonIndex
   int dead_man_switch;
   int fast_mode;
   int slow_mode;
+
+  int e_stop_reset;
+  int e_stop_trigger;
+  int enable_e_stop_reset;
 };
 
 class Joy2TwistNode : public rclcpp::Node
@@ -35,18 +43,31 @@ class Joy2TwistNode : public rclcpp::Node
   void declare_parameters();
   void load_parameters();
 
+  void e_stop_cb(const std::shared_ptr<MsgBool> bool_msg);
   void joy_cb(const std::shared_ptr<MsgJoy> joy_msg);
   void convert_joy_to_twist(const std::shared_ptr<MsgJoy> joy_msg, MsgTwist & twist_msg);
   std::pair<float, float> determine_velocity_factor(const std::shared_ptr<MsgJoy> joy_msg);
+  void call_trigger_service(const rclcpp::Client<SrvTrigger>::SharedPtr & client) const;
+  void trigger_service_cb(
+    const rclcpp::Client<SrvTrigger>::SharedFuture & future,
+    const std::string & service_name) const;
 
   std::map<std::string, float> linear_velocity_factors_;
   std::map<std::string, float> angular_velocity_factors_;
 
   ButtonIndex button_index_;
   bool driving_mode_;
+  bool e_stop_present_;
+  bool e_stop_state_;
+  std::string e_stop_topic_;
+  std::string e_stop_reset_srv_;
+  std::string e_stop_trigger_srv_;
 
+  rclcpp::Subscription<MsgBool>::SharedPtr e_stop_sub_;
   rclcpp::Subscription<MsgJoy>::SharedPtr joy_sub_;
   rclcpp::Publisher<MsgTwist>::SharedPtr twist_pub_;
+  rclcpp::Client<SrvTrigger>::SharedPtr e_stop_reset_client_;
+  rclcpp::Client<SrvTrigger>::SharedPtr e_stop_trigger_client_;
 };
 
 static constexpr char FAST[]{"fast"};

diff --git a/joy2twist/package.xml b/joy2twist/package.xml
@@ -15,6 +15,8 @@
   <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>joy_linux</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>

diff --git a/joy2twist/src/joy2twist_node.cpp b/joy2twist/src/joy2twist_node.cpp
@@ -2,15 +2,26 @@
 
 namespace joy2twist
 {
+using std::placeholders::_1;
+
 Joy2TwistNode::Joy2TwistNode() : Node("joy2twist_node")
 {
   using namespace std::placeholders;
 
   declare_parameters();
   load_parameters();
 
-  joy_sub_ = create_subscription<MsgJoy>("joy", rclcpp::SensorDataQoS(), std::bind(&Joy2TwistNode::joy_cb, this, _1));
-  twist_pub_ = create_publisher<MsgTwist>("cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable());
+  joy_sub_ = create_subscription<MsgJoy>(
+    "joy", rclcpp::SensorDataQoS(), std::bind(&Joy2TwistNode::joy_cb, this, _1));
+  twist_pub_ = create_publisher<MsgTwist>(
+    "cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable());
+
+  if (e_stop_present_) {
+    e_stop_sub_ = this->create_subscription<MsgBool>(
+      e_stop_topic_, rclcpp::SystemDefaultsQoS(), std::bind(&Joy2TwistNode::e_stop_cb, this, _1));
+    e_stop_reset_client_ = this->create_client<SrvTrigger>(e_stop_reset_srv_);
+    e_stop_trigger_client_ = this->create_client<SrvTrigger>(e_stop_trigger_srv_);
+  }
 
   RCLCPP_INFO(get_logger(), "Initialized node!");
 }
@@ -23,12 +34,21 @@ void Joy2TwistNode::declare_parameters()
   this->declare_parameter<float>("angular_velocity_factor.fast", 1.0);
   this->declare_parameter<float>("angular_velocity_factor.regular", 0.5);
   this->declare_parameter<float>("angular_velocity_factor.slow", 0.2);
-  this->declare_parameter<int>("button_index_map.axis.angular_z", 0);
-  this->declare_parameter<int>("button_index_map.axis.linear_x", 3);
-  this->declare_parameter<int>("button_index_map.axis.linear_y", 2);
+
+  this->declare_parameter<bool>("e_stop.present", false);
+  this->declare_parameter<std::string>("e_stop.topic", "e_stop");
+  this->declare_parameter<std::string>("e_stop.reset_srv", "e_stop_reset");
+  this->declare_parameter<std::string>("e_stop.trigger_srv", "e_stop_trigger");
+
+  this->declare_parameter<int>("button_index_map.axis.angular_z", 2);
+  this->declare_parameter<int>("button_index_map.axis.linear_x", 1);
+  this->declare_parameter<int>("button_index_map.axis.linear_y", 0);
   this->declare_parameter<int>("button_index_map.dead_man_switch", 4);
   this->declare_parameter<int>("button_index_map.fast_mode", 7);
   this->declare_parameter<int>("button_index_map.slow_mode", 5);
+  this->declare_parameter<int>("button_index_map.e_stop_reset", 1);
+  this->declare_parameter<int>("button_index_map.e_stop_trigger", 2);
+  this->declare_parameter<int>("button_index_map.enable_e_stop_reset", 6);
 }
 
 void Joy2TwistNode::load_parameters()
@@ -39,17 +59,42 @@ void Joy2TwistNode::load_parameters()
   this->get_parameter<float>("angular_velocity_factor.fast", angular_velocity_factors_[FAST]);
   this->get_parameter<float>("angular_velocity_factor.regular", angular_velocity_factors_[REGULAR]);
   this->get_parameter<float>("angular_velocity_factor.slow", angular_velocity_factors_[SLOW]);
+
+  this->get_parameter<bool>("e_stop.present", e_stop_present_);
+  this->get_parameter<std::string>("e_stop.topic", e_stop_topic_);
+  this->get_parameter<std::string>("e_stop.reset_srv", e_stop_reset_srv_);
+  this->get_parameter<std::string>("e_stop.trigger_srv", e_stop_trigger_srv_);
+
   this->get_parameter<int>("button_index_map.axis.angular_z", button_index_.angular_z);
   this->get_parameter<int>("button_index_map.axis.linear_x", button_index_.linear_x);
   this->get_parameter<int>("button_index_map.axis.linear_y", button_index_.linear_y);
   this->get_parameter<int>("button_index_map.dead_man_switch", button_index_.dead_man_switch);
   this->get_parameter<int>("button_index_map.fast_mode", button_index_.fast_mode);
   this->get_parameter<int>("button_index_map.slow_mode", button_index_.slow_mode);
+  this->get_parameter<int>("button_index_map.e_stop_reset", button_index_.e_stop_reset);
+  this->get_parameter<int>("button_index_map.e_stop_trigger", button_index_.e_stop_trigger);
+  this->get_parameter<int>(
+    "button_index_map.enable_e_stop_reset", button_index_.enable_e_stop_reset);
 }
 
+void Joy2TwistNode::e_stop_cb(const MsgBool::SharedPtr bool_msg) { e_stop_state_ = bool_msg->data; }
+
 void Joy2TwistNode::joy_cb(const MsgJoy::SharedPtr joy_msg)
 {
   MsgTwist twist_msg;
+
+  if (e_stop_present_) {
+    if (joy_msg->buttons.at(button_index_.e_stop_trigger) && !e_stop_state_) {
+      // Stop the robot before trying to call the e-stop trigger service
+      twist_pub_->publish(twist_msg);
+      call_trigger_service(e_stop_trigger_client_);
+    } else if (
+      joy_msg->buttons.at(button_index_.enable_e_stop_reset) &&
+      joy_msg->buttons.at(button_index_.e_stop_reset) && e_stop_state_) {
+      call_trigger_service(e_stop_reset_client_);
+    }
+  }
+
   if (joy_msg->buttons.at(button_index_.dead_man_switch)) {
     driving_mode_ = true;
     convert_joy_to_twist(joy_msg, twist_msg);
@@ -86,4 +131,31 @@ std::pair<float, float> Joy2TwistNode::determine_velocity_factor(const MsgJoy::S
   return std::make_pair(linear_velocity_factor, angular_velocity_factor);
 }
 
+void Joy2TwistNode::call_trigger_service(const rclcpp::Client<SrvTrigger>::SharedPtr & client) const
+{
+  if (!client->wait_for_service(std::chrono::milliseconds(2000))) {
+    RCLCPP_ERROR(this->get_logger(), "Can't contact %s service", client->get_service_name());
+    return;
+  }
+
+  const auto request = std::make_shared<SrvTrigger::Request>();
+
+  client->async_send_request(request, [&](const rclcpp::Client<SrvTrigger>::SharedFuture future) {
+    trigger_service_cb(future, client->get_service_name());
+  });
+}
+
+void Joy2TwistNode::trigger_service_cb(
+  const rclcpp::Client<SrvTrigger>::SharedFuture & future, const std::string & service_name) const
+{
+  if (!future.get()->success) {
+    RCLCPP_ERROR(
+      this->get_logger(), "Failed to call %s service: %s", service_name.c_str(),
+      future.get()->message.c_str());
+    return;
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Successfully called %s service", service_name.c_str());
+}
+
 }  // namespace joy2twist