E-stop functionalities to Gazebo simulation

panther/panther_simulation.repos

@@ -22,4 +22,4 @@ repositories:
   husarion_gz_worlds:
     type: git
     url: https://github.com/husarion/husarion_gz_worlds.git
-    version: bba5074af5eabf404850245a70a1d60192912a3b
+    version: 9d514a09c74bca2afbfba76cf2c87134918bbf97

panther_description/CMakeLists.txt

@@ -7,6 +7,6 @@ install(DIRECTORY config launch meshes rviz urdf
         DESTINATION share/${PROJECT_NAME})
 
 ament_environment_hooks(
-  "${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in")
+  "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in")
 
 ament_package()

panther_description/urdf/gazebo.urdf.xacro

@@ -42,6 +42,9 @@
         <parameters>${config_file}</parameters>
         <ros>
           <namespace>${namespace}</namespace>
+          <remapping>gz_ros2_control/e_stop:=hardware/e_stop</remapping>
+          <remapping>gz_ros2_control/e_stop_reset:=hardware/e_stop_reset</remapping>
+          <remapping>gz_ros2_control/e_stop_trigger:=hardware/e_stop_trigger</remapping>
           <remapping>imu_broadcaster/imu:=imu/data</remapping>
           <remapping>drive_controller/cmd_vel_unstamped:=cmd_vel</remapping>
           <remapping>drive_controller/odom:=odometry/wheels</remapping>

panther_description/urdf/panther_macro.urdf.xacro

@@ -62,7 +62,9 @@
     <ros2_control name="${ns}panther_system" type="system">
       <hardware>
         <xacro:if value="${use_sim}">
-          <plugin>ign_ros2_control/IgnitionSystem</plugin>
+          <plugin>panther_gazebo/PantherSystem</plugin>
+          <param name="e_stop_publish_frequency">100</param>
+          <param name="e_stop_initial_state">true</param>
         </xacro:if>
 
         <xacro:unless value="$(arg use_sim)">

panther_gazebo/CMakeLists.txt

@@ -5,29 +5,86 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(PACKAGE_DEPENDENCIES ament_cmake ignition-transport11 ignition-common4
-                         ignition-msgs8 panther_utils yaml-cpp)
+set(PACKAGE_DEPENDENCIES
+    ament_cmake
+    hardware_interface
+    ignition-common4
+    ignition-gazebo6
+    ignition-msgs8
+    ignition-plugin1
+    ignition-transport11
+    ign_ros2_control
+    pluginlib
+    panther_utils
+    rclcpp
+    std_msgs
+    std_srvs
+    yaml-cpp)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
 endforeach()
 
-include_directories(include)
+find_package(Qt5 REQUIRED COMPONENTS Core Quick QuickControls2)
 
-set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR})
-set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR})
-set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR})
+include_directories(include ${Qt5Core_INCLUDE_DIRS} ${Qt5Qml_INCLUDE_DIRS}
+                    ${Qt5Quick_INCLUDE_DIRS} ${Qt5QuickControls2_INCLUDE_DIRS})
 
 add_executable(gz_led_strip_manager src/main.cpp src/gz_led_strip_manager.cpp
                                     src/gz_led_strip.cpp)
 ament_target_dependencies(gz_led_strip_manager panther_utils)
 target_link_libraries(
-  gz_led_strip_manager ignition-transport${IGN_TRANSPORT_VER}::core
-  ignition-msgs${IGN_MSGS_VER}
-  ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} yaml-cpp)
+  gz_led_strip_manager ignition-transport11::core ignition-msgs8
+  ignition-common4::ignition-common4 yaml-cpp)
+
+add_library(panther_hardware_plugins SHARED src/gz_panther_system.cpp)
+ament_target_dependencies(
+  panther_hardware_plugins
+  rclcpp_lifecycle
+  hardware_interface
+  rclcpp
+  std_msgs
+  std_srvs
+  ign_ros2_control)
+target_link_libraries(panther_hardware_plugins ignition-gazebo6::core)
+
+set(CMAKE_AUTOMOC ON)
+qt5_add_resources(resources_rcc src/gui/Estop.qrc)
+
+add_library(Estop SHARED include/panther_gazebo/gui/Estop.hpp src/gui/Estop.cpp
+                         ${resources_rcc})
+ament_target_dependencies(Estop ignition-common4 ignition-gazebo6
+                          ignition-plugin1 rclcpp std_srvs)
+target_link_libraries(Estop ${Qt5Core_LIBRARIES} ${Qt5Qml_LIBRARIES}
+                      ${Qt5Quick_LIBRARIES} ${Qt5QuickControls2_LIBRARIES})
 
 install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME})
 
+install(
+  TARGETS panther_hardware_plugins
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib)
+
+install(
+  TARGETS Estop
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib)
+
 install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+
+pluginlib_export_plugin_description_file(ign_ros2_control
+                                         panther_hardware_plugins.xml)
+
+ament_environment_hooks(
+  "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in")
+
 ament_package()

panther_gazebo/README.md

@@ -14,3 +14,39 @@ The package contains a launch file and source files used to run the robot simula
 - [`battery_plugin_config.yaml`](./config/battery_plugin_config.yaml): Simulated LinearBatteryPlugin configuration.
 - [`gz_bridge.yaml`](./config/gz_bridge.yaml): Specify data to exchange between ROS and Gazebo simulation.
 - [`led_strips.yaml`](./config/led_strips.yaml): Configure properties of led strips in simulation to animate lights.
+- [`teleop_with_estop.config`](./config/teleop_with_estop.config): Gazebo layout configuration file, which add E-Stop and Teleop widgets.
+
+## ROS Nodes
+
+### Estop
+
+`Estop` is a Gazebo GUI plugin responsible for easy and convenient changing of the robot's E-stop state.
+
+#### Publishers
+
+- `gz_ros2_control/e_stop` [*std_msgs/Bool*]: Current E-stop state.
+
+#### Service Servers
+
+- `gz_ros2_control/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop.
+- `gz_ros2_control/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop.
+
+### PantherSystem
+
+Plugin based on `ign_system` is responsible for handling sensor interfaces (only IMU for now) and sending requests for joints compatible with `ros2_control`. Plugin also adds E-Stop support.
+
+#### Publishers
+
+- `hardware/e_stop` [*std_msgs/Bool*]: Current E-stop state.
+
+#### Service Servers
+
+- `hardware/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop.
+- `hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop.
+
+#### Parameters
+
+Required parameters are defined when including the interface in the URDF (you can check out [panther_macro.urdf.xacro](../panther_description/urdf/panther_macro.urdf.xacro)).
+
+- `e_stop_publish_frequency` [*float*, default: **100**]: Frequency of publication of e-stop status information.
+- `e_stop_initial_state` [*bool*, default: **true**]: Initial state of E-stop.

panther_gazebo/config/gz_bridge.yaml

@@ -17,14 +17,12 @@
   gz_type_name: ignition.msgs.Twist
   direction: GZ_TO_ROS
 
-- ros_topic_name: lights/channel_1_frame
-  gz_topic_name: lights/channel_1_frame
+- topic_name: lights/channel_1_frame
   ros_type_name: sensor_msgs/msg/Image
   gz_type_name: ignition.msgs.Image
   direction: ROS_TO_GZ
 
-- ros_topic_name: lights/channel_2_frame
-  gz_topic_name: lights/channel_2_frame
+- topic_name: lights/channel_2_frame
   ros_type_name: sensor_msgs/msg/Image
   gz_type_name: ignition.msgs.Image
   direction: ROS_TO_GZ

panther_gazebo/config/led_strips.yaml

@@ -1,5 +1,5 @@
 chanel_1:
-  frequency: 15  # Setting to high frequency caused lags in the simulation
+  frequency: 12  # Setting to high frequency caused lags in the simulation
   world_name: husarion_world
   parent_link: panther
   position: [0.362, 0.0, 0.201]
@@ -10,7 +10,7 @@ chanel_1:
   number_of_leds: 46
 
 chanel_2:
-  frequency: 15  # Setting to high frequency caused lags in the simulation
+  frequency: 12  # Setting to high frequency caused lags in the simulation
   world_name: husarion_world
   parent_link: panther
   position: [-0.362, 0.0, 0.201]