diff --git a/.clang-format b/.clang-format index ca3cb2f..e76a1de 100644 --- a/.clang-format +++ b/.clang-format @@ -14,4 +14,8 @@ ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle -ReflowComments: false \ No newline at end of file +ReflowComments: true +IncludeBlocks: Preserve +AlignOperands: true +PenaltyBreakAssignment: 21 +PenaltyBreakBeforeFirstCallParameter: 1 diff --git a/README.md b/README.md index 98772f5..687a312 100644 --- 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 -``` \ No newline at end of file +``` diff --git a/demo/single_robot/compose.panther.yaml b/demo/single_robot/compose.panther.yaml index 79618f9..3acb3a5 100644 --- 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 index e2218e2..a6dc3b2 100644 --- 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 index 06181b1..3ec0933 100644 --- 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 index c50af4b..a9d7552 100644 --- a/joy2twist/include/joy2twist/joy2twist_node.hpp +++ b/joy2twist/include/joy2twist/joy2twist_node.hpp @@ -9,11 +9,15 @@ #include #include +#include +#include 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 bool_msg); void joy_cb(const std::shared_ptr joy_msg); void convert_joy_to_twist(const std::shared_ptr joy_msg, MsgTwist & twist_msg); std::pair determine_velocity_factor(const std::shared_ptr joy_msg); + void call_trigger_service(const rclcpp::Client::SharedPtr & client) const; + void trigger_service_cb( + const rclcpp::Client::SharedFuture & future, + const std::string & service_name) const; std::map linear_velocity_factors_; std::map 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::SharedPtr e_stop_sub_; rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Client::SharedPtr e_stop_reset_client_; + rclcpp::Client::SharedPtr e_stop_trigger_client_; }; static constexpr char FAST[]{"fast"}; diff --git a/joy2twist/package.xml b/joy2twist/package.xml index 58a9ddb..b0a8fc8 100644 --- a/joy2twist/package.xml +++ b/joy2twist/package.xml @@ -15,6 +15,8 @@ rclcpp geometry_msgs sensor_msgs + std_msgs + std_srvs joy_linux ament_cmake_gtest diff --git a/joy2twist/src/joy2twist_node.cpp b/joy2twist/src/joy2twist_node.cpp index 8a6bc28..cf94c99 100644 --- a/joy2twist/src/joy2twist_node.cpp +++ b/joy2twist/src/joy2twist_node.cpp @@ -2,6 +2,8 @@ namespace joy2twist { +using std::placeholders::_1; + Joy2TwistNode::Joy2TwistNode() : Node("joy2twist_node") { using namespace std::placeholders; @@ -9,8 +11,17 @@ Joy2TwistNode::Joy2TwistNode() : Node("joy2twist_node") declare_parameters(); load_parameters(); - joy_sub_ = create_subscription("joy", rclcpp::SensorDataQoS(), std::bind(&Joy2TwistNode::joy_cb, this, _1)); - twist_pub_ = create_publisher("cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable()); + joy_sub_ = create_subscription( + "joy", rclcpp::SensorDataQoS(), std::bind(&Joy2TwistNode::joy_cb, this, _1)); + twist_pub_ = create_publisher( + "cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable()); + + if (e_stop_present_) { + e_stop_sub_ = this->create_subscription( + e_stop_topic_, rclcpp::SystemDefaultsQoS(), std::bind(&Joy2TwistNode::e_stop_cb, this, _1)); + e_stop_reset_client_ = this->create_client(e_stop_reset_srv_); + e_stop_trigger_client_ = this->create_client(e_stop_trigger_srv_); + } RCLCPP_INFO(get_logger(), "Initialized node!"); } @@ -23,12 +34,21 @@ void Joy2TwistNode::declare_parameters() this->declare_parameter("angular_velocity_factor.fast", 1.0); this->declare_parameter("angular_velocity_factor.regular", 0.5); this->declare_parameter("angular_velocity_factor.slow", 0.2); - this->declare_parameter("button_index_map.axis.angular_z", 0); - this->declare_parameter("button_index_map.axis.linear_x", 3); - this->declare_parameter("button_index_map.axis.linear_y", 2); + + this->declare_parameter("e_stop.present", false); + this->declare_parameter("e_stop.topic", "e_stop"); + this->declare_parameter("e_stop.reset_srv", "e_stop_reset"); + this->declare_parameter("e_stop.trigger_srv", "e_stop_trigger"); + + this->declare_parameter("button_index_map.axis.angular_z", 2); + this->declare_parameter("button_index_map.axis.linear_x", 1); + this->declare_parameter("button_index_map.axis.linear_y", 0); this->declare_parameter("button_index_map.dead_man_switch", 4); this->declare_parameter("button_index_map.fast_mode", 7); this->declare_parameter("button_index_map.slow_mode", 5); + this->declare_parameter("button_index_map.e_stop_reset", 1); + this->declare_parameter("button_index_map.e_stop_trigger", 2); + this->declare_parameter("button_index_map.enable_e_stop_reset", 6); } void Joy2TwistNode::load_parameters() @@ -39,17 +59,42 @@ void Joy2TwistNode::load_parameters() this->get_parameter("angular_velocity_factor.fast", angular_velocity_factors_[FAST]); this->get_parameter("angular_velocity_factor.regular", angular_velocity_factors_[REGULAR]); this->get_parameter("angular_velocity_factor.slow", angular_velocity_factors_[SLOW]); + + this->get_parameter("e_stop.present", e_stop_present_); + this->get_parameter("e_stop.topic", e_stop_topic_); + this->get_parameter("e_stop.reset_srv", e_stop_reset_srv_); + this->get_parameter("e_stop.trigger_srv", e_stop_trigger_srv_); + this->get_parameter("button_index_map.axis.angular_z", button_index_.angular_z); this->get_parameter("button_index_map.axis.linear_x", button_index_.linear_x); this->get_parameter("button_index_map.axis.linear_y", button_index_.linear_y); this->get_parameter("button_index_map.dead_man_switch", button_index_.dead_man_switch); this->get_parameter("button_index_map.fast_mode", button_index_.fast_mode); this->get_parameter("button_index_map.slow_mode", button_index_.slow_mode); + this->get_parameter("button_index_map.e_stop_reset", button_index_.e_stop_reset); + this->get_parameter("button_index_map.e_stop_trigger", button_index_.e_stop_trigger); + this->get_parameter( + "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 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::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(); + + client->async_send_request(request, [&](const rclcpp::Client::SharedFuture future) { + trigger_service_cb(future, client->get_service_name()); + }); +} + +void Joy2TwistNode::trigger_service_cb( + const rclcpp::Client::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