From 25da047db3313f70b0d19382d86c27155ba75b55 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 25 Jul 2024 18:18:41 +0200 Subject: [PATCH 01/22] Namespace refactor --- panther_description/urdf/gazebo.urdf.xacro | 14 ++------------ panther_description/urdf/panther_macro.urdf.xacro | 7 +------ 2 files changed, 3 insertions(+), 18 deletions(-) diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro index b8b2afdbb..4d315fd29 100644 --- a/panther_description/urdf/gazebo.urdf.xacro +++ b/panther_description/urdf/gazebo.urdf.xacro @@ -6,12 +6,7 @@ - - - - - - + @@ -65,12 +60,7 @@ - - - - - - + diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro index 7ff4192e2..01c2764bf 100644 --- a/panther_description/urdf/panther_macro.urdf.xacro +++ b/panther_description/urdf/panther_macro.urdf.xacro @@ -16,12 +16,7 @@ imu_rot_y:=0.0 namespace:=''"> - - - - - - + From 415a28e7c567dabafb5919d512a9403c754f40a0 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Thu, 22 Aug 2024 14:33:18 +0200 Subject: [PATCH 02/22] Add pre-commit workflow (#395) --- .github/workflows/pre-commit.yaml | 11 +++++++++++ .pre-commit-config.yaml | 8 ++++---- ROS_API.md | 12 ++++++------ panther_localization/launch/nmea_navsat.launch.py | 1 - 4 files changed, 21 insertions(+), 11 deletions(-) create mode 100644 .github/workflows/pre-commit.yaml diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 000000000..08f7311e1 --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,11 @@ +--- +name: Pre-Commit + +on: + push: + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 421c74955..c426d2101 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -34,7 +34,7 @@ repos: - id: cmake-format - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.6 + rev: v18.1.8 hooks: - id: clang-format @@ -63,13 +63,13 @@ repos: args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100'] - repo: https://github.com/psf/black - rev: 24.4.2 + rev: 24.8.0 hooks: - id: black args: ["--line-length=99"] - repo: https://github.com/PyCQA/flake8 - rev: 7.1.0 + rev: 7.1.1 hooks: - id: flake8 args: @@ -94,7 +94,7 @@ repos: exclude: ^.*\/CHANGELOG\.rst/.*$ - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.8.0 + rev: v0.10.0 hooks: - id: prettier-package-xml - id: sort-package-xml diff --git a/ROS_API.md b/ROS_API.md index 8b663c8c5..9497bd700 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -1,12 +1,12 @@ # ROS API -> [!IMPORTANT] -> **Beta Release** -> -> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur. -> +> [!IMPORTANT] +> **Beta Release** +> +> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur. +> > We would greatly appreciate your feedback regarding the Panther ROS 2 driver. You can reach us in the following ways: -> +> > - By email at: [support@husarion.com](mailto:support@husarion.com) > - Via our community forum: [Husarion Community](https://community.husarion.com) > - By submitting an issue request on: [GitHub](https://github.com/husarion/panther_ros/issues) diff --git a/panther_localization/launch/nmea_navsat.launch.py b/panther_localization/launch/nmea_navsat.launch.py index f5995b2d2..585438f26 100644 --- a/panther_localization/launch/nmea_navsat.launch.py +++ b/panther_localization/launch/nmea_navsat.launch.py @@ -23,7 +23,6 @@ ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from nav2_common.launch import ReplaceString def generate_launch_description(): From 22499989de1ae5730e0e290fcfb63e8abbbea238 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Wed, 28 Aug 2024 11:51:20 +0200 Subject: [PATCH 03/22] 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 | 2 +- panther_description/urdf/gazebo.urdf.xacro | 21 +- .../urdf/panther_macro.urdf.xacro | 4 +- panther_gazebo/CMakeLists.txt | 47 +-- panther_gazebo/README.md | 27 +- panther_gazebo/config/gz_bridge.yaml | 6 +- panther_gazebo/config/led_strips.yaml | 21 -- panther_gazebo/hooks/panther_gazebo.sh.in | 1 + .../{plugins => gui}/e_stop.hpp | 6 +- .../include/panther_gazebo/gz_led_strip.hpp | 109 ------- .../panther_gazebo/gz_led_strip_manager.hpp | 49 --- .../include/panther_gazebo/led_strip.hpp | 149 +++++++++ .../launch/simulate_robot.launch.py | 8 - panther_gazebo/launch/simulation.launch.py | 2 +- panther_gazebo/package.xml | 7 +- panther_gazebo/src/{plugins => gui}/EStop.qml | 0 panther_gazebo/src/{plugins => gui}/EStop.qrc | 0 .../src/{plugins => gui}/e_stop.cpp | 2 +- panther_gazebo/src/gz_led_strip.cpp | 215 ------------- panther_gazebo/src/gz_led_strip_manager.cpp | 68 ----- panther_gazebo/src/led_strip.cpp | 282 ++++++++++++++++++ panther_gazebo/src/main.cpp | 48 --- 22 files changed, 517 insertions(+), 557 deletions(-) delete mode 100644 panther_gazebo/config/led_strips.yaml rename panther_gazebo/include/panther_gazebo/{plugins => gui}/e_stop.hpp (93%) delete mode 100644 panther_gazebo/include/panther_gazebo/gz_led_strip.hpp delete mode 100644 panther_gazebo/include/panther_gazebo/gz_led_strip_manager.hpp create mode 100644 panther_gazebo/include/panther_gazebo/led_strip.hpp rename panther_gazebo/src/{plugins => gui}/EStop.qml (100%) rename panther_gazebo/src/{plugins => gui}/EStop.qrc (100%) rename panther_gazebo/src/{plugins => gui}/e_stop.cpp (98%) delete mode 100644 panther_gazebo/src/gz_led_strip.cpp delete mode 100644 panther_gazebo/src/gz_led_strip_manager.cpp create mode 100644 panther_gazebo/src/led_strip.cpp delete mode 100644 panther_gazebo/src/main.cpp diff --git a/panther_battery/README.md b/panther_battery/README.md index 6b0e8b447..dc151104f 100644 --- a/panther_battery/README.md +++ b/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. diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro index e34de17a8..29e3558f0 100644 --- a/panther_description/urdf/gazebo.urdf.xacro +++ b/panther_description/urdf/gazebo.urdf.xacro @@ -106,9 +106,18 @@ - - + + + + ${name} + ${topic} + ${namespace} + 10 + 0.5 + 0.015 + + true @@ -116,12 +125,18 @@ 1.0 1.0 1.0 1.0 1.0 1.0 1 0 0 - 1.0 + 0.5 1.0 2.0 0.4 + + 20.0 + 1.0 + 1.0 + 1.0 + diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro index fc6da39ee..8d2a83207 100644 --- a/panther_description/urdf/panther_macro.urdf.xacro +++ b/panther_description/urdf/panther_macro.urdf.xacro @@ -185,8 +185,8 @@ - - + + diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt index 31dc77d04..cae0aaa39 100644 --- a/panther_gazebo/CMakeLists.txt +++ b/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) diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md index 36bf7bb30..a4d3bb85a 100644 --- a/panther_gazebo/README.md +++ b/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. diff --git a/panther_gazebo/config/gz_bridge.yaml b/panther_gazebo/config/gz_bridge.yaml index 8b6c2328c..d7f8114bf 100644 --- a/panther_gazebo/config/gz_bridge.yaml +++ b/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: /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: /lights/channel_2_frame ros_type_name: sensor_msgs/msg/Image gz_type_name: ignition.msgs.Image direction: ROS_TO_GZ diff --git a/panther_gazebo/config/led_strips.yaml b/panther_gazebo/config/led_strips.yaml deleted file mode 100644 index c23e06e5f..000000000 --- a/panther_gazebo/config/led_strips.yaml +++ /dev/null @@ -1,21 +0,0 @@ -chanel_1: - 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] - orientation: [0.0, 0.0, 0.0] - led_strip_width: 0.5 - topic: lights/channel_1_frame - light_name: lights_channel_1 - number_of_leds: 46 - -chanel_2: - 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] - orientation: [0.0, 0.0, 0.0] - led_strip_width: 0.5 - topic: lights/channel_2_frame - light_name: lights_channel_2 - number_of_leds: 46 diff --git a/panther_gazebo/hooks/panther_gazebo.sh.in b/panther_gazebo/hooks/panther_gazebo.sh.in index 2aaf5d29b..5e75c5eb0 100644 --- a/panther_gazebo/hooks/panther_gazebo.sh.in +++ b/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" diff --git a/panther_gazebo/include/panther_gazebo/plugins/e_stop.hpp b/panther_gazebo/include/panther_gazebo/gui/e_stop.hpp similarity index 93% rename from panther_gazebo/include/panther_gazebo/plugins/e_stop.hpp rename to panther_gazebo/include/panther_gazebo/gui/e_stop.hpp index 2edc75636..055db7e16 100644 --- a/panther_gazebo/include/panther_gazebo/plugins/e_stop.hpp +++ b/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 @@ -61,4 +61,4 @@ protected slots: }; } // namespace panther_gazebo -#endif // PANTHER_GAZEBO_PLUGINS_ESTOP_HPP_ +#endif // PANTHER_GAZEBO_GUI_E_STOP_HPP_ diff --git a/panther_gazebo/include/panther_gazebo/gz_led_strip.hpp b/panther_gazebo/include/panther_gazebo/gz_led_strip.hpp deleted file mode 100644 index cf177ff8f..000000000 --- a/panther_gazebo/include/panther_gazebo/gz_led_strip.hpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PANTHER_GAZEBO_GZ_LED_STRIP_HPP_ -#define PANTHER_GAZEBO_GZ_LED_STRIP_HPP_ - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -using namespace std::chrono_literals; - -/** - * @brief Structure to hold properties of each LED channel - */ -struct ChannelProperties -{ - uint8_t frequency; - std::string world_name; - std::string parent_link; - std::vector position; - std::vector orientation; - double led_strip_width; - std::string topic; - std::string light_name; - unsigned int number_of_leds; -}; - -struct RGBAColor -{ - float r; - float g; - float b; - float a; -}; - -/** - * @brief Class to manage an LED strip in a Gazebo simulation - */ -class LEDStrip -{ -public: - /** - * @brief Construct a new LEDStrip object - * - * @param channel_properties Properties of the LED channel - */ - LEDStrip(ChannelProperties channel_properties); - ~LEDStrip(); - -private: - void ImageCallback(const gz::msgs::Image & msg); - void MsgValidation(const gz::msgs::Image & msg); - - /** - * @brief Manage color of robot simulated lights based on the image message - */ - void ManageLights(const gz::msgs::Image & msg); - - /** - * @brief Manage color of robot LED strip based on the image message - */ - void ManageVisualization(const gz::msgs::Image & msg); - RGBAColor CalculateMeanRGBA(const gz::msgs::Image & msg); - - /** - * @brief Sending a message to change the color of the light - * - * @param rgba The RGBA color to publish - */ - void PublishLight(const RGBAColor & rgba); - - /** - * @brief Create a marker element (single LED from LED Strip) - * - * @param marker The pointer to marker to create - * @param id The unique ID of the marker (if not unique, the marker will replace the existing one) - */ - void CreateMarker(ignition::msgs::Marker * marker, const int id); - void SetMarkerColor(gz::msgs::Marker * marker, const RGBAColor & rgba); - - static unsigned int first_free_available_marker_idx_; - const int first_led_marker_idx_; - - ChannelProperties channel_properties_; - std::shared_ptr node_; - std::shared_ptr light_pub_; -}; - -#endif // PANTHER_GAZEBO_GZ_LED_STRIP_HPP_ diff --git a/panther_gazebo/include/panther_gazebo/gz_led_strip_manager.hpp b/panther_gazebo/include/panther_gazebo/gz_led_strip_manager.hpp deleted file mode 100644 index 3494ed580..000000000 --- a/panther_gazebo/include/panther_gazebo/gz_led_strip_manager.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PANTHER_GAZEBO_GZ_LED_STRIP_MANAGER_HPP_ -#define PANTHER_GAZEBO_GZ_LED_STRIP_MANAGER_HPP_ - -#include -#include - -#include - -#include "panther_gazebo/gz_led_strip.hpp" -#include "panther_utils/yaml_utils.hpp" - -/** - * @brief Class to manage multiple LED strip object in Gazebo simulation based on provided - * configuration - */ -class LEDStripManager -{ -public: - explicit LEDStripManager(const std::string & config_file); - void LoadConfig(const std::string & config_file); - - /** - * @brief Create as many LED strips as defined in the configuration file. - */ - void CreateLEDStrips(); - - template - void AssignProperty(const YAML::Node & channel_args, T & property, const std::string & key); - -private: - YAML::Node config_; - std::list led_strips_; -}; - -#endif // PANTHER_GAZEBO_GZ_LED_STRIP_MANAGER_HPP_ diff --git a/panther_gazebo/include/panther_gazebo/led_strip.hpp b/panther_gazebo/include/panther_gazebo/led_strip.hpp new file mode 100644 index 000000000..0f0945ea6 --- /dev/null +++ b/panther_gazebo/include/panther_gazebo/led_strip.hpp @@ -0,0 +1,149 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_GAZEBO_LED_STRIP_HPP_ +#define PANTHER_GAZEBO_LED_STRIP_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace panther_gazebo +{ + +/** + * @brief Class to manage an LED strip in a Gazebo simulation based on received image. + */ +class LEDStrip : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate +{ +public: + /** + * @brief Configures the LED strip. This function fill up parameters and light_cmd_ based on URDF. + * Inherit from gz::sim::ISystemConfigure. More information can be found in the [Gazebo + * documentation](https://gazebosim.org/api/gazebo/6/createsystemplugins.html). + * + * @param id The entity ID of the model. + * @param sdf The SDF element of the model. + * @param ecm The entity component manager. + * @param eventMgr The event manager. + * + * @exception std::runtime_error if the entity is not a model. + */ + void Configure( + const gz::sim::Entity & id, const std::shared_ptr & sdf, + gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & eventMgr) override; + + /** + * @brief Displays lights and markers, with specified by URDF frequency. Inherit from + * gz::sim::ISystemPreUpdate. More information can be found in the [Gazebo + * documentation](https://gazebosim.org/api/gazebo/6/createsystemplugins.html). + * + * @param info The update information. + * @param ecm The entity component manager. + */ + void PreUpdate(const gz::sim::UpdateInfo & info, gz::sim::EntityComponentManager & ecm) override; + +private: + /** + * @brief Parse parameters from the URDF file + * + * @param sdf The SDF element of the model. + * @exception std::runtime_error if the light_name or topic parameter is missing. + */ + void ParseParameters(const std::shared_ptr & sdf); + + /** + * @brief Return Light command based on light configuration specified in URDF file Light + * properties + * + * @param ecm The entity component manager. + * @return Light command message + * @exception std::runtime_error if the light entity is not found. + */ + gz::msgs::Light SetupLightCmd(gz::sim::EntityComponentManager & ecm); + + /** + * @brief Convert sdf::Light (configuration from URDF) to gz::msgs::Light (command msg) + * + * @param light_sdf Light SDF configuration + * @return Light command message + */ + gz::msgs::Light CreateLightMsgFromSdf(const sdf::Light & light_sdf); + void ImageCallback(const gz::msgs::Image & msg); + bool IsEncodingValid(const gz::msgs::Image & msg); + gz::math::Color CalculateMeanColor(const gz::msgs::Image & msg); + + /** + * @brief Manage color of robot simulated lights based on the image message + * + * @param ecm The entity component manager. + * @param image The image message + */ + void VisualizeLights(gz::sim::EntityComponentManager & ecm, const gz::msgs::Image & image); + + /** + * @brief Manage color of robot LED strip based on the image message + * + * @param image The image message + * @param light_pose The pose of the light + */ + void VisualizeMarkers(const gz::msgs::Image & image, const gz::math::Pose3d & light_pose); + + /** + * @brief Create a marker element (single LED from LED Strip) + * + * @param id The unique ID of the marker (if not unique, the marker will replace the existing one) + * @param pose The pose of the marker + * @param color The color of the marker + * @param size The size of the marker + */ + void CreateMarker( + const uint id, const gz::math::Pose3d pose, const gz::math::Color & color, + const gz::math::Vector3d size); + + std::string light_name_; + std::string image_topic_; + std::string ns_ = ""; + double frequency_ = 10.0; + double marker_width_ = 1.0; + double marker_height_ = 1.0; + + bool new_image_available_ = false; + gz::msgs::Light light_cmd_; + realtime_tools::RealtimeBox last_image_; + gz::sim::Entity light_entity_{gz::sim::kNullEntity}; + gz::transport::Node node_; + std::chrono::steady_clock::duration last_update_time_{std::chrono::seconds( + 1)}; // Avoid initialization errors when the robot is not yet spawned on the scene. +}; + +} // namespace panther_gazebo + +#endif // PANTHER_GAZEBO_LED_STRIP_HPP_ diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index 55d120be8..ad7252e7a 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -121,13 +121,6 @@ def generate_launch_description(): condition=UnlessCondition(PythonExpression(["'", namespace, "' == ''"])), ) - gz_led_strip_manager = Node( - package="panther_gazebo", - executable="gz_led_strip_manager", - namespace=namespace, - arguments=["--config-file", gz_led_strip_config], - ) - controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -203,7 +196,6 @@ def generate_launch_description(): spawn_robot_launch, lights_launch, manager_launch, - gz_led_strip_manager, controller_launch, ekf_launch, simulate_components, diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py index 348fbaee6..04c192d74 100644 --- a/panther_gazebo/launch/simulation.launch.py +++ b/panther_gazebo/launch/simulation.launch.py @@ -56,7 +56,7 @@ def generate_launch_description(): [FindPackageShare("husarion_gz_worlds"), "launch", "gz_sim.launch.py"] ) ), - launch_arguments={"gz_gui": namespaced_gz_gui}.items(), + launch_arguments={"gz_gui": namespaced_gz_gui, "gz_log_level": "1"}.items(), ) simulate_robots = IncludeLaunchDescription( diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index af1ab5412..89e7baaac 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -19,14 +19,13 @@ hardware_interface ign_ros2_control - panther_utils pluginlib rclcpp rclcpp_lifecycle - sensor_msgs + realtime_tools + ros_gz_sim std_msgs std_srvs - yaml-cpp controller_manager husarion_gz_worlds @@ -38,10 +37,10 @@ panther_lights panther_localization panther_manager + panther_utils robot_state_publisher ros_components_description ros_gz_bridge - ros_gz_sim tf2_ros