LEDStrip plugin to Gazebo (#391)

* Save work

* save work

* Save work

* Fix light

* Format

* Clean up

* Clean up

* Documentation

* Simplify

* Rename folder

* Clean up

* pre-commit

* Add simple brief

* Review minor changes

* Jakub suggestions

* Prevent from initial error and warn logs

* Improve light property

* Add suggestions

* Add suggestions

* Throw instead of errormsg

panther_battery/README.md

@@ -22,7 +22,7 @@ Publishes battery state read from ADC unit for Panther version 1.2 and above, or
 - `battery/charging_status` [*panther_msgs/ChargingStatus*]: Battery charging status.
 - `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Battery diagnostic messages.
 
-#### Subscribes
+#### Subscribers
 
 - `hardware/io_state` [*panther_msgs/IOState*]: Current state of IO.
 - `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.

panther_description/urdf/gazebo.urdf.xacro

@@ -106,22 +106,37 @@
     </gazebo>
   </xacro:macro>
 
-
   <!-- Lights -->
-  <xacro:macro name="light" params="reference_frame name namespace:=''">
+  <xacro:macro name="light" params="reference_frame name topic namespace:=''">
+    <gazebo>
+      <plugin filename="LEDStrip" name="panther_gazebo::LEDStrip">
+        <light_name>${name}</light_name>
+        <topic>${topic}</topic>
+        <namespace>${namespace}</namespace>
+        <frequency>10</frequency>
+        <width>0.5</width>
+        <height>0.015</height>
+      </plugin>
+    </gazebo>
     <gazebo reference="${reference_frame}">
       <light name="${name}" type="spot">
         <cast_shadows>true</cast_shadows>
         <visualize>0</visualize>
         <diffuse>1.0 1.0 1.0</diffuse>
         <specular>1.0 1.0 1.0</specular>
         <direction>1 0 0</direction>
-        <intensity>1.0</intensity>
+        <intensity>0.5</intensity>
         <spot>
           <inner_angle>1.0</inner_angle>
           <outer_angle>2.0</outer_angle>
           <falloff>0.4</falloff>
         </spot>
+        <attenuation>
+          <range>20.0</range>
+          <constant>1.0</constant>
+          <linear>1.0</linear>
+          <quadratic>1.0</quadratic>
+        </attenuation>
       </light>
     </gazebo>
   </xacro:macro>

panther_description/urdf/panther_macro.urdf.xacro

@@ -185,8 +185,8 @@
       </xacro:if>
       <xacro:gazebo.controller config_file="${controller_config_file}" namespace="${namespace}" />
       <xacro:gazebo.imu reference_frame="imu_link" namespace="${namespace}" />
-      <xacro:gazebo.light reference_frame="lights_channel_1_link" name="lights_channel_1" namespace="${namespace}" />
-      <xacro:gazebo.light reference_frame="lights_channel_2_link" name="lights_channel_2" namespace="${namespace}" />
+      <xacro:gazebo.light reference_frame="lights_channel_1_link" name="lights_channel_1" topic="lights/channel_1_frame" namespace="${namespace}" />
+      <xacro:gazebo.light reference_frame="lights_channel_2_link" name="lights_channel_2" topic="lights/channel_2_frame" namespace="${namespace}" />
     </xacro:if>
   </xacro:macro>
 

panther_gazebo/CMakeLists.txt

@@ -8,18 +8,16 @@ endif()
 set(PACKAGE_DEPENDENCIES
     ament_cmake
     hardware_interface
-    ignition-common4
     ignition-gazebo6
     ignition-msgs8
     ignition-plugin1
     ignition-transport11
     ign_ros2_control
     pluginlib
-    panther_utils
     rclcpp
+    realtime_tools
     std_msgs
-    std_srvs
-    yaml-cpp)
+    std_srvs)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
@@ -30,13 +28,6 @@ find_package(Qt5 REQUIRED COMPONENTS Core Quick QuickControls2)
 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-transport11::core ignition-msgs8
-  ignition-common4::ignition-common4 yaml-cpp)
-
 add_library(panther_simulation_plugins SHARED src/gz_panther_system.cpp)
 ament_target_dependencies(
   panther_simulation_plugins
@@ -46,19 +37,27 @@ ament_target_dependencies(
   rclcpp_lifecycle
   std_msgs
   std_srvs)
-target_link_libraries(panther_simulation_plugins ignition-gazebo6::core)
+target_link_libraries(panther_simulation_plugins ignition-gazebo6)
 
 set(CMAKE_AUTOMOC ON)
-qt5_add_resources(resources_rcc src/plugins/EStop.qrc)
-
-add_library(EStop SHARED include/panther_gazebo/plugins/e_stop.hpp
-                         src/plugins/e_stop.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})
+qt5_add_resources(resources_rcc src/gui/EStop.qrc)
 
-install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME})
+add_library(EStop SHARED include/panther_gazebo/gui/e_stop.hpp
+                         src/gui/e_stop.cpp ${resources_rcc})
+ament_target_dependencies(EStop rclcpp std_srvs)
+target_link_libraries(
+  EStop
+  ignition-gazebo6
+  ignition-plugin1
+  ${Qt5Core_LIBRARIES}
+  ${Qt5Qml_LIBRARIES}
+  ${Qt5Quick_LIBRARIES}
+  ${Qt5QuickControls2_LIBRARIES})
+
+add_library(LEDStrip SHARED src/led_strip.cpp)
+ament_target_dependencies(LEDStrip realtime_tools)
+target_link_libraries(LEDStrip ignition-gazebo6 ignition-msgs8 ignition-plugin1
+                      ignition-transport11)
 
 install(
   TARGETS panther_simulation_plugins
@@ -72,6 +71,12 @@ install(
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib)
 
+install(
+  TARGETS LEDStrip
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib)
+
 install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)

panther_gazebo/README.md

@@ -13,7 +13,6 @@ 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 adds E-Stop and Teleop widgets.
 
 ## ROS Nodes
@@ -48,3 +47,29 @@ Plugin based on `ign_system` is responsible for handling sensor interfaces (only
 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_initial_state` [*bool*, default: **true**]: Initial state of E-stop.
+
+### LEDStrip
+
+`LEDStrip` is a Gazebo System plugin responsible for visualizing light and displaying markers based on the data received from a `gz::msgs::Image` message.
+
+> [!NOTE]
+> The topics and services mentioned below are related to Gazebo interfaces, not ROS interfaces.
+
+#### Subscribers
+
+- `{topic}` [*gz::msgs::Image*]: Subscribes to an image message for visualization. The topic is specified via a parameter.
+
+#### Service Servers
+
+- `/marker` [*gz::msgs::Marker*]: Service to request markers for visualizing the received image.
+
+#### Parameters
+
+The following parameters are required when including this interface in the URDF (you can refer to the [gazebo.urdf.xacro](../panther_description/urdf/gazebo.urdf.xacro) file for details).
+
+- `light_name` [*string*, default: **""**]: The name of the light entity. The visualization will be attached to this entity.
+- `topic` [*string*, default: **""**]: The name of the topic from which the Image message is received.
+- `namespace` [*string*, default: **""**]: Specifies the namespace to differentiate topics and models in scenarios with multiple robots.
+- `frequency` [*double*, default: **10.0**]: Defines the frequency at which the animation is updated.
+- `width` [*double*, default: **1.0**]: Specifies the width (y-axis) of the visualization array.
+- `height` [*double*, default: **1.0**]: Specifies the height (z-axis) of the visualization array.

panther_gazebo/config/gz_bridge.yaml

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

panther_gazebo/hooks/panther_gazebo.sh.in

@@ -1 +1,2 @@
 ament_prepend_unique_value IGN_GUI_PLUGIN_PATH "@CMAKE_INSTALL_PREFIX@/lib"
+ament_prepend_unique_value IGN_GAZEBO_SYSTEM_PLUGIN_PATH= "@CMAKE_INSTALL_PREFIX@/lib"

panther_gazebo/include/panther_gazebo/plugins/e_stop.hpp → panther_gazebo/include/panther_gazebo/gui/e_stop.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PANTHER_GAZEBO_PLUGINS_ESTOP_HPP_
-#define PANTHER_GAZEBO_PLUGINS_ESTOP_HPP_
+#ifndef PANTHER_GAZEBO_GUI_E_STOP_HPP_
+#define PANTHER_GAZEBO_GUI_E_STOP_HPP_
 
 #include <string>
 
@@ -61,4 +61,4 @@ protected slots:
 };
 }  // namespace panther_gazebo
 
-#endif  // PANTHER_GAZEBO_PLUGINS_ESTOP_HPP_
+#endif  // PANTHER_GAZEBO_GUI_E_STOP_HPP_