diff --git a/.docs/panther_ros2_api.drawio.svg b/.docs/panther_ros2_api.drawio.svg new file mode 100644 index 000000000..29cefa8b9 --- /dev/null +++ b/.docs/panther_ros2_api.drawio.svg @@ -0,0 +1,4 @@ + + + +
panther_localization
panther_localization
panther_controller
panther_controller
panther_lights
panther_lights
panther_hardware_interfaces
panther_hardware_interfaces
panther_battery
panther_battery
panther_manager
panther_manager
panther_diagnostics
panther_diagnostics
PantherSystem
PantherSystem
hardware_controller
hardware_controller
GPIOController
GPIOController
MotorsController
MotorsController
CANopenController
CANopenController
RoboteqDriver
RoboteqDriver
CAN
CAN
EStopManager
EStopManager
battery_driver
battery_driver
RPi GPIO
RPi GPIO
ADC
ADC
imu_broadcaster
imu_broadcaster
joint_state_broadcaster
joint_state_broadcaster
drive_controller
drive_controller
controller_manager
controller_manager
PantherImuSensor

PantherImuSensor
IMU
IMU
ekf_filter
ekf_filter
ImuFilter
ImuFilter
Spatial
Spatial
imu/data
imu/data
odometry/wheels
odometry/wheels
odometry/filtered
odometry/filtered
joint_states
joint_states
cmd_vel
cmd_vel
dynamic_joint_states
dynamic_joint_states
 hardware/e_stop
 hardware/e_stop
hardware/motor_controllers_state
hardware/motor_controllers_state
 hardware/io_state
 hardware/io_state
hardware/e_stop_trigger
hardware/e_stop_trigger
hardware/e_stop_reset
hardware/e_stop_reset
hardware/<hw_component>_enable
hardware/<hw_component>_enable
robot_state_publisher
robot_state_publisher
robot_description
robot_description
/tf
/tf
/tf_static
/tf_static
/tf
/tf
battery/battery_status
battery/battery_status
Bumper Lights
Bumper Lights
lights_driver
lights_driver
lights_controller
lights_controller
lights/channel_1_frame
lights/channel_1_frame
lights/channel_2_frame
lights/channel_2_frame
lights/set_animation
lights/set_animation
lights/set_brightness
lights/set_brightness
safety_manager
safety_manager
system_monitor
system_monitor
system_status
system_status
lights_manager
lights_manager
hardware/led_control_enable
hardware/led_control_enable
/tf
/tf
battery/charging_status
battery/charging_status
GPIODriver
GPIODriver
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
Legend:
Legend:
Nodes
Nodes
Plugin
Plugin
Objects
Objects
Topics
Topics
Services
Services
Data Transfer
Data Trans...Text is not SVG - cannot display diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index de27bccb5..0da411369 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1,7 +1,14 @@ ### Description -[Summary of the changes] +- -### Modifications +### Requirements -- +- [ ] Code style guidelines followed +- [ ] Documentation updated + +### Tests 🧪 + +- [ ] Robot +- [ ] Container +- [ ] Simulation 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 66c65de5b..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 @@ -51,7 +51,7 @@ repos: "--ignore-words-list", "ned" # north, east, down (NED) ] - exclude_types: [rst] + exclude_types: [rst, svg] language: python types: [text] @@ -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/README.md b/README.md index f738d8fc4..633e44f3e 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,11 @@ ROS 2 packages for Panther autonomous mobile robot [![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit)](https://github.com/pre-commit/pre-commit) + + + Panther preview + + ## Quick start ### Create workspace @@ -66,6 +71,55 @@ Simulation: ros2 launch panther_gazebo simulation.launch.py ``` +### Launch Arguments + +Launch arguments are largely common to both simulation and physical robot. However, there is a group of arguments that apply only to hardware or only to the simulator. Below is a legend to the tables with all launch arguments. + +| Symbol | Meaning | +| ------ | ---------------------------- | +| 🤖 | Available for physical robot | +| 🖥️ | Available in simulation | + +| | Argument | Description
***Type:*** `Default` | +| --- | ---------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🖥️ | `add_wheel_joints` | Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information.
***bool:*** `False` | +| 🖥️ | `add_world_transform` | Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots).
***bool:*** `False` | +| 🖥️ | `battery_config_path` | Path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only.
***string:*** `None` | +| 🤖🖥️ | `components_config_path` | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options).
***string:*** [`components.yaml`](./panther_description/config/components.yaml) | +| 🤖🖥️ | `controller_config_path` | Path to controller configuration file. A path to custom configuration can be specified here.
***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/) | +| 🤖 | `disable_manager` | Enable or disable manager_bt_node.
***bool:*** `False` | +| 🤖🖥️ | `fuse_gps` | Include GPS for data fusion.
***bool:*** `False` | +| 🖥️ | `gz_bridge_config_path` | Path to the parameter_bridge configuration file.
***string:*** [`gz_bridge.yaml`](./panther_gazebo/config/gz_bridge.yaml) | +| 🖥️ | `gz_gui` | Run simulation with specific GUI layout.
***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config) | +| 🖥️ | `gz_headless_mode` | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations.
***bool:*** `False` | +| 🖥️ | `gz_log_level` | Adjust the level of console output.
***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`) | +| 🖥️ | `gz_world` | Absolute path to SDF world file.
***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf) | +| 🤖 | `launch_nmea_gps` | Whether to launch the NMEA NavSat driver node. Advisable when the robot is equipped with the [ANT02](https://husarion.com/manuals/panther/panther-options/#ant02---wi-fi--lte--gps).
***bool:*** `False` | +| 🤖🖥️ | `led_config_file` | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
***string:*** [`led_config.yaml`](./panther_lights/config/led_config.yaml) | +| 🤖🖥️ | `lights_bt_project_path` | Path to BehaviorTree project file, responsible for lights management.
***string:*** [`PantherLightsBT.btproj`](./panther_manager/behavior_trees/PantherLightsBT.btproj) | +| 🤖🖥️ | `localization_config_path` | Specify the path to the localization configuration file.
***string:*** [`relative_localization.yaml`](./panther_localization/config/relative_localization.yaml) | +| 🤖🖥️ | `localization_mode` | Specifies the localization mode:
- 'relative' `odometry/filtered` data is relative to the initial position and orientation.
- 'enu' `odometry/filtered` data is relative to initial position and ENU (East North Up) orientation.
***string:*** `relative` (choices: `relative`, `enu`) | +| 🤖🖥️ | `namespace` | Add namespace to all launched nodes.
***string:*** `env(ROBOT_NAMESPACE)` | +| 🤖🖥️ | `publish_robot_state` | Whether to publish the default Panther robot description.
***bool:*** `True` | +| 🖥️ | `robots` | The list of the robots spawned in the simulation e.g. `robots:='robot1={x: 1.0, y: -2.0}; robot2={x: 1.0, y: -4.0}'`
***string:*** `''` | +| 🤖🖥️ | `safety_bt_project_path` | Path to BehaviorTree project file, responsible for safety and shutdown management.
***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj) | +| 🤖🖥️ | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown.
***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml) | +| 🤖🖥️ | `use_ekf` | Enable or disable EKF.
***bool:*** `True` | +| 🤖🖥️ | `use_sim` | Whether simulation is used.
***bool:*** `False` | +| 🤖🖥️ | `user_led_animations_file` | Path to a YAML file with a description of the user-defined animations.
***string:*** `''` | +| 🤖🖥️ | `wheel_config_path` | Path to wheel configuration file.
***string:*** [`{wheel_type}.yaml`](./panther_description/config) | +| 🤖🖥️ | `wheel_type` | Type of wheel. If you choose a value from the preset options ('WH01', 'WH02', 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path' parameters. For custom wheels, please define these parameters to point to files that accurately describe the custom wheels.
***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) | +| 🖥️ | `x` | Initial robot position in the global 'x' axis.
***float:*** `0.0` | +| 🖥️ | `y` | Initial robot position in the global 'y' axis.
***float:***` -2.0` | +| 🖥️ | `z` | Initial robot position in the global 'z' axis.
***float:*** `0.2` | +| 🖥️ | `roll` | Initial robot 'roll' orientation.
***float:*** `0.0` | +| 🖥️ | `pitch` | Initial robot 'pitch' orientation.
***float:*** `0.0` | +| 🖥️ | `yaw` | Initial robot 'yaw' orientation.
***float:*** `0.0` | + +> [!TIP] +> +> To read the arguments for individual packages, add the `-s` flag to the `ros2 launch` command (e.g. `ros2 launch panther_bringup bringup.launch.py ​​-s`) + ## Developer Info ### Setup pre-commit diff --git a/ROS_API.md b/ROS_API.md new file mode 100644 index 000000000..1a92bdc24 --- /dev/null +++ b/ROS_API.md @@ -0,0 +1,127 @@ +# 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. +> +> 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) + +## ROS 2 System Design + +This section describes the ROS packages in the Panther ROS system. These packages are located in the [panther_ros](https://github.com/husarion/panther_ros) GitHub repository. + +> [!NOTE] +> **Differences in ROS System** +> +> ROS 2 nodes differs slightly between **Panther v1.06** and **Panther v1.2+**. This is caused by internal hardware differences. Despite that, the ROS API was kept as closely matched between those revisions as possible and should be transparent in most of the use cases. + + + +The default way to communicate with Panther's hardware is via the Robot Operating System (ROS). All the drivers were written in ROS 2 framework. The ROS API is provided by ROS packages found in the GitHub repository [husarion/panther_ros](https://github.com/husarion/panther_ros). These packages are responsible for accessing the hardware components of the robot. + +The graph below represents Panther's ROS system. Some topics and services have been excluded from the graph for the sake of clarity. + +![Panther ROS 2 API Diagram](.docs/panther_ros2_api.drawio.svg) + +## ROS Interfaces + +Below is information about the physical robot API. For the simulation, topics and services are identical to the physical robot, but due to certain limitations, not all interfaces are present in the simulation. + +| Symbol | Meaning | +| ------ | ------------------------------- | +| 🤖 | Available for physical robot | +| 🖥️ | Available in simulation | +| ⚙️ | Requires specific configuration | + +### Nodes + +| | Node name | Description | +| --- | ------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🤖 | `battery_driver` | Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot.
[*panther_batter/battery_driver_node*](./panther_battery/include/panther_battery/battery_driver_node.hpp) | +| 🤖🖥️ | `controller_manager` | The Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. This node manages the: `imu_broadcaster`, `joint_state_broadcaster`, `drive_controller`.
*[controller_manager/controller_manager](https://github.com/ros-controls/ros2_control/blob/master/controller_manager)* | +| 🤖🖥️ | `drive_controller` | Manages mobile robots with a differential or mecanum drive depending on the configuration. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it.
*[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller) or [mecanum_drive_controller/mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller)* | +| 🤖🖥️ | `ekf_filter` | The Extended Kalman Filter node is designed to fuse odometry data from various sources, including wheel encoders, IMU, and GPS.
*[robot_localization/ekf_filter](https://github.com/cra-ros-pkg/robot_localization)* | +| 🤖 | `hardware_controller` | Plugin responsible for communicating with engine controllers via the CAN bus and providing E-Stop functionalities.
*[panther_hardware_interfaces/PantherSystem](./panther_hardware_interfaces/src/panther_system/)* | +| 🤖 | `gps` | Node responsible for parsing NMEA strings and publishing standard ROS NavSat message types.
*[nmea_navsat_driver/nmea_navsat_driver](https://github.com/ros-drivers/nmea_navsat_driver/tree/ros2/src/libnmea_navsat_driver)* | +| 🖥️ | `gz_ros2_control` | Responsible for integrating the ros2_control controller architecture with the Gazebo simulator.
[gz_ros2_control/gz_ros2_control](https://github.com/ros-controls/gz_ros2_control/tree/master/gz_ros2_control/src) | +| 🖥️ | `gz_estop_gui` | The node is part of the Gazebo GUI plugin, enabling easy E-stop state modifications directly within the simulation.
[panther_gazebo/EStop](./panther_gazebo/src/gui/e_stop.cpp) | +| 🤖🖥️ | `imu_broadcaster` | Publishes readings of IMU sensors.
*[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)* | +| 🤖🖥️ | `joint_state_broadcaster` | Reads all state interfaces and reports them on specific topics.
*[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)* | +| 🤖🖥️ | `lights_container` | Node for managing ROS components. This node manages: `lights_controller`, `lights_driver`.
[*rclcpp_components/component_container*](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_components) | +| 🤖🖥️ | `lights_controller` | This node is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights.
[*panther_lights/LightsControllerNode*](./panther_lights/include/panther_lights/lights_controller_node.hpp) | +| 🤖 | `lights_driver` | This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights.
[*panther_lights/LightsDriverNode*](./panther_lights/include/panther_lights/lights_driver_node.hpp) | +| 🤖🖥️ | `lights_manager` | Node responsible for managing Bumper Lights animation scheduling.
[panther_manager/lights_manager](./panther_manager/include/panther_manager/lights_manager_node.hpp) | +| 🤖🖥️⚙️ | `navsat_transform` | It converts raw GPS data into odometry data and publishes corrected GPS positions based on sensor data at a higher frequency.
*[robot_localization/navsat_transform](https://github.com/cra-ros-pkg/robot_localization)* | +| 🖥️ | `panther_base_gz_bridge` | Convert and transmit data between ROS and Gazebo
*[ros_gz_bridge/parameter_bridge](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge)* | +| 🤖🖥️ | `robot_state_publisher` | Broadcasts a robot's state to tf2 using a provided URDF model and joint states. It updates the model and broadcasts poses for fixed and movable joints to tf2 topics.
*[robot_state_publisher/robot_state_publisher](https://github.com/ros/robot_state_publisher)* | +| 🤖 | `safety_manager` | Node responsible for managing safety features, and software shutdown of components.
*[panther_manager/safety_manager_node](./panther_manager/include/panther_manager/safety_manager_node.hpp)* | +| 🤖 | `system_monitor` | Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and CPU temperature.
*[panther_diagnostic/system_monitor_node](./panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp)* | + +### Topics + +| | Topic | Description | +| --- | ---------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🤖🖥️ | `battery/battery_status` | Mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | +| 🤖 | `battery/charging_status` | Battery charging status value.
[*panther_msgs/ChargingStatus*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `cmd_vel` | Command velocity value.
[*geometry_msgs/Twist*](https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html) | +| 🤖🖥️ | `diagnostics` | Diagnostic data.
[*diagnostic_msgs/DiagnosticArray*](https://docs.ros2.org/latest/api/diagnostic_msgs/msg/DiagnosticArray.html) | +| 🤖🖥️ | `dynamic_joint_states` | Information about the state of various movable joints in a robotic system.
[*control_msgs/DynamicJointState*](https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/DynamicJointState.msg) | +| 🤖🖥️⚙️ | `gps/filtered` | Filtered GPS position after fusing odometry data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | +| 🤖🖥️⚙️ | `gps/fix` | Raw GPS data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | +| 🤖 | `gps/time_reference` | The timestamp from the GPS device.
[*sensor_msgs/TimeReference*](https://docs.ros2.org/latest/api/sensor_msgs/msg/TimeReference.html) | +| 🤖 | `gps/vel` | Velocity output from the GPS device.
[*geometry_msgs/TwistStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html) | +| 🤖🖥️ | `hardware/e_stop` | Current E-stop state.
[*std_msgs/Bool*](https://docs.ros2.org/latest/api/std_msgs/msg/Bool.html). | +| 🤖 | `hardware/io_state` | Current IO state.
[*panther_msgs/IOState*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `hardware/motor_controllers_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
[*panther_msgs/DriverState*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `imu/data` | Filtered IMU data.
[*sensor_msgs/Imu*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html) | +| 🤖🖥️ | `joint_states` | Provides information about the state of various joints in a robotic system.
[*sensor_msgs/JointState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html) | +| 🤖🖥️ | `lights/channel_1_frame` | Frame to be displayed on robot Front Bumper Lights.
[*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) | +| 🤖🖥️ | `lights/channel_2_frame` | Frame to be displayed on robot Rear Bumper Lights.
[*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) | +| 🤖🖥️ | `localization/set_pose` | Sets the pose of the EKF node.
[*geometry_msgs/PoseWithCovarianceStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseWithCovarianceStamped.html) | +| 🤖🖥️ | `odometry/filtered` | Contains information about the filtered position and orientation. When `localization_mode` is `relative`, the position and orientation are relative to the starting point. When `localization_mode` is `enu`, the orientation is relative to the east-north-up (ENU) coordinates.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | +| 🤖🖥️ | `odometry/wheels` | Robot odometry calculated from wheels.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | +| 🤖🖥️ | `robot_description` | Contains information about robot description from URDF file.
[*std_msgs/String*](https://docs.ros2.org/latest/api/std_msgs/msg/String.html) | +| 🤖 | `system_status` | State of the system, including Built-in Computer's CPU temperature and load.
[*panther_msgs/SystemStatus*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `tf` | Transforms of robot system.
[*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html) | +| 🤖🖥️ | `tf_static` | Static transforms of robot system.
[*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html) | + +#### Hidden topics + +| | Topic | Description | +| --- | ------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🤖 | `_battery/battery_1_status_raw` | First battery raw state.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | +| 🤖 | `_battery/battery_2_status_raw` | Second battery raw state. Published if second battery detected.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | +| 🤖 | `_gps/heading` | Not supported for current configuration.
[*geometry_msgs/QuaternionStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/QuaternionStamped.html)| +| 🤖🖥️⚙️ | `_odometry/gps` | Transformed raw GPS data to odometry format.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | + +### Services + +| | Service | Description | +| --- | ------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| 🤖🖥️ | `controller_manager/configure_controller` | Manage lifecycle transition.
[controller_manager_msgs/srv/ConfigureController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_controller_types` | Output the available controller types and their base classes.
[controller_manager_msgs/srv/ListControllerTypes](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_controllers` | Output the list of loaded controllers, their type and status.
[controller_manager_msgs/srv/ListControllers](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_hardware_components` | Output the list of available hardware components.
[controller_manager_msgs/srv/ListHardwareComponents](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_hardware_interfaces` | Output the list of available command and state interfaces.
[controller_manager_msgs/srv/ListHardwareInterfaces](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/load_controller` | Load a controller in a controller manager.
[controller_manager_msgs/srv/LoadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/reload_controller_libraries` | Reload controller libraries.
[controller_manager_msgs/srv/ReloadControllerLibraries](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/set_hardware_component_state` | Adjust the state of the hardware component.
[controller_manager_msgs/srv/SetHardwareComponentState](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/switch_controller` | Switch controllers in a controller manager.
[controller_manager_msgs/srv/SwitchController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/unload_controller` | Unload a controller in a controller manager.
[controller_manager_msgs/srv/UnloadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖 | `hardware/aux_power_enable` | Enables or disables AUX power.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/charger_enable` | Enables or disables external charger.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/digital_power_enable` | Enables or disables digital power.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖🖥️ | `hardware/e_stop_reset` | Resets E-stop.
[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) | +| 🤖🖥️ | `hardware/e_stop_trigger` | Triggers E-stop.
[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) | +| 🤖 | `hardware/fan_enable` | Enables or disables fan.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/motor_power_enable` | Enables or disables motor power.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖🖥️ | `lights/set_animation` | Sets LED animation.
[panther_msgs/srv/SetLEDAnimation](https://github.com/husarion/panther_msgs) | +| 🤖 | `lights/set_brightness` | Sets global LED brightness, value ranges from **0.0** to **1.0**.
[panther_msgs/SetLEDBrightness](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `localization/enable` | Enable EKF node.
[std_srvs/srv/Empty](https://docs.ros2.org/latest/api/std_srvs/srv/Empty.html) | +| 🤖🖥️ | `localization/set_pose` | Set pose of EKF node.
[robot_localization/srv/SetPose](https://github.com/cra-ros-pkg/robot_localization/tree/ros2) | +| 🤖🖥️ | `localization/toggle` | Toggle filter processing in the EKF node.
[robot_localization/srv/ToggleFilterProcessing](https://github.com/cra-ros-pkg/robot_localization/tree/ros2) | diff --git a/panther/CHANGELOG.rst b/panther/CHANGELOG.rst index 268ba8437..f889df6f9 100644 --- a/panther/CHANGELOG.rst +++ b/panther/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package panther ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.1 (2024-09-05) +------------------ +* Merge branch 'ros2-devel' into ros2-ns-refactor +* Merge branch 'ros2-devel' of https://github.com/husarion/panther_ros into ros2-add-nmea-gps +* Merge pull request `#383 `_ from husarion/ros-sim-estop +* Merge remote-tracking branch 'origin/ros2-devel' into ros2-testing-poc +* Ros2 estop sim gui (`#384 `_) +* Merge branch 'ros2-devel' into ros2-ns-refactor +* Contributors: Dawid Kmak, KmakD, pawelirh, rafal-gorecki + 2.1.0 (2024-08-02) ------------------ * Merge pull request `#375 `_ from husarion/hotfix-handle-can-timeout diff --git a/panther/package.xml b/panther/package.xml index 9a41a8daa..f849f5671 100644 --- a/panther/package.xml +++ b/panther/package.xml @@ -2,7 +2,7 @@ panther - 2.1.0 + 2.1.1 Meta package that contains all packages of Panther Husarion Apache License 2.0 diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index b3e2233f7..83df44fa3 100644 --- a/panther/panther_simulation.repos +++ b/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 diff --git a/panther_battery/CHANGELOG.rst b/panther_battery/CHANGELOG.rst index 2c3f3c32d..de4e0a10e 100644 --- a/panther_battery/CHANGELOG.rst +++ b/panther_battery/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package panther_battery ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.1 (2024-09-05) +------------------ +* LEDStrip plugin to Gazebo (`#391 `_) +* Merge branch 'ros2-devel' into ros2-ns-refactor +* Merge pull request `#349 `_ from husarion/ros2-testing-poc +* Merge branch 'ros2-devel' of https://github.com/husarion/panther_ros into ros2-add-nmea-gps +* Merge remote-tracking branch 'origin/ros2-devel' into ros2-testing-poc +* Merge pull request `#386 `_ from husarion/ros2-unify-filenames +* Merge remote-tracking branch 'origin/ros2-devel' into ros2-unify-filenames +* Ros2 battery estimation (`#376 `_) +* Rename battery exec +* Rename battery driver files +* Reorganize files in panther_battery +* Ros2 estop sim gui (`#384 `_) +* Merge branch 'ros2-devel' into ros2-ns-refactor +* unify CMakeLists.txt files (`#381 `_) +* unify CMakeLists.txt files +* New format of documentation (`#369 `_) +* Contributors: BOOTCFG, Dawid, Dawid Kmak, KmakD, Paweł Irzyk, pawelirh, rafal-gorecki + 2.1.0 (2024-08-02) ------------------ * Merge pull request `#375 `_ from husarion/hotfix-handle-can-timeout diff --git a/panther_battery/CMakeLists.txt b/panther_battery/CMakeLists.txt index 9bc58a551..4e12bab87 100644 --- a/panther_battery/CMakeLists.txt +++ b/panther_battery/CMakeLists.txt @@ -5,29 +5,27 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(panther_msgs REQUIRED) -find_package(panther_utils REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) +set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater panther_msgs + panther_utils rclcpp sensor_msgs) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() include_directories(include ${panther_utils_INCLUDE_DIRS}) add_executable( - battery_node + battery_driver_node src/main.cpp - src/battery_node.cpp - src/adc_battery.cpp - src/roboteq_battery.cpp - src/battery_publisher.cpp - src/dual_battery_publisher.cpp - src/single_battery_publisher.cpp) - -ament_target_dependencies(battery_node diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + src/battery_driver_node.cpp + src/battery/adc_battery.cpp + src/battery/roboteq_battery.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp) +ament_target_dependencies(battery_driver_node ${PACKAGE_DEPENDENCIES}) -install(TARGETS battery_node DESTINATION lib/${PROJECT_NAME}) +install(TARGETS battery_driver_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) @@ -41,7 +39,7 @@ if(BUILD_TESTING) PUBLIC $ $) - ament_add_gtest(${PROJECT_NAME}_test_battery test/test_battery.cpp) + ament_add_gtest(${PROJECT_NAME}_test_battery test/battery/test_battery.cpp) target_include_directories( ${PROJECT_NAME}_test_battery PUBLIC $ @@ -49,8 +47,8 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_battery rclcpp sensor_msgs panther_msgs) - ament_add_gtest(${PROJECT_NAME}_test_adc_battery test/test_adc_battery.cpp - src/adc_battery.cpp) + ament_add_gtest(${PROJECT_NAME}_test_adc_battery + test/battery/test_adc_battery.cpp src/battery/adc_battery.cpp) target_include_directories( ${PROJECT_NAME}_test_adc_battery PUBLIC $ @@ -58,8 +56,9 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_adc_battery rclcpp sensor_msgs panther_msgs) - ament_add_gtest(${PROJECT_NAME}_test_roboteq_battery - test/test_roboteq_battery.cpp src/roboteq_battery.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_roboteq_battery test/battery/test_roboteq_battery.cpp + src/battery/roboteq_battery.cpp) target_include_directories( ${PROJECT_NAME}_test_roboteq_battery PUBLIC $ @@ -67,90 +66,90 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_roboteq_battery panther_msgs rclcpp sensor_msgs) - ament_add_gtest(${PROJECT_NAME}_test_battery_publisher - test/test_battery_publisher.cpp src/battery_publisher.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_battery_publisher + test/battery_publisher/test_battery_publisher.cpp + src/battery_publisher/battery_publisher.cpp) target_include_directories( ${PROJECT_NAME}_test_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_publisher diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_single_battery_publisher - test/test_single_battery_publisher.cpp src/adc_battery.cpp - src/battery_publisher.cpp src/single_battery_publisher.cpp) + test/battery_publisher/test_single_battery_publisher.cpp + src/battery/adc_battery.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp) target_include_directories( ${PROJECT_NAME}_test_single_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_single_battery_publisher diagnostic_updater - panther_msgs panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_single_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_dual_battery_publisher - test/test_dual_battery_publisher.cpp src/adc_battery.cpp - src/battery_publisher.cpp src/dual_battery_publisher.cpp) + test/battery_publisher/test_dual_battery_publisher.cpp + src/battery/adc_battery.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp) target_include_directories( ${PROJECT_NAME}_test_dual_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_dual_battery_publisher diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_dual_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( - ${PROJECT_NAME}_test_battery_node - test/test_battery_node.cpp - src/adc_battery.cpp - src/battery_node.cpp - src/battery_publisher.cpp - src/dual_battery_publisher.cpp - src/single_battery_publisher.cpp - src/roboteq_battery.cpp) + ${PROJECT_NAME}_test_battery_driver_node_adc_dual + test/test_battery_driver_node_adc_dual.cpp + src/battery/adc_battery.cpp + src/battery_driver_node.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp + src/battery/roboteq_battery.cpp) target_include_directories( - ${PROJECT_NAME}_test_battery_node + ${PROJECT_NAME}_test_battery_driver_node_adc_dual PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_dual + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( - ${PROJECT_NAME}_test_battery_node_roboteq - test/test_battery_node_roboteq.cpp - src/adc_battery.cpp - src/battery_node.cpp - src/battery_publisher.cpp - src/dual_battery_publisher.cpp - src/single_battery_publisher.cpp - src/roboteq_battery.cpp) + ${PROJECT_NAME}_test_battery_driver_node_adc_single + test/test_battery_driver_node_adc_single.cpp + src/battery/adc_battery.cpp + src/battery_driver_node.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp + src/battery/roboteq_battery.cpp) target_include_directories( - ${PROJECT_NAME}_test_battery_node_roboteq + ${PROJECT_NAME}_test_battery_driver_node_adc_single PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node_roboteq diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_single + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( - ${PROJECT_NAME}_test_battery_node_dual_bat - test/test_battery_node_dual_bat.cpp - src/adc_battery.cpp - src/battery_node.cpp - src/battery_publisher.cpp - src/dual_battery_publisher.cpp - src/single_battery_publisher.cpp - src/roboteq_battery.cpp) + ${PROJECT_NAME}_test_battery_driver_node_roboteq + test/test_battery_driver_node_roboteq.cpp + src/battery/adc_battery.cpp + src/battery_driver_node.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp + src/battery/roboteq_battery.cpp) target_include_directories( - ${PROJECT_NAME}_test_battery_node_dual_bat + ${PROJECT_NAME}_test_battery_driver_node_roboteq PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node_dual_bat diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_roboteq + ${PACKAGE_DEPENDENCIES}) endif() diff --git a/panther_battery/README.md b/panther_battery/README.md index ed6bab6d0..dc151104f 100644 --- a/panther_battery/README.md +++ b/panther_battery/README.md @@ -1,64 +1,39 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_battery -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - -Package containing nodes monitoring and publishing the internal battery state of the Husarion Panther robot. - -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) +The package containing nodes monitoring and publishing the internal battery state of the Husarion Panther robot. -## ROS Nodes +## Launch Files -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +This package contains: -### battery_driver +- `battery.launch.py`: Responsible for activating battery node, which dealing with reading and publishing battery data. -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) +## ROS Nodes -Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot. +### battery_driver_node -[//]: # (ROS_API_NODE_DESCRIPTION_END) +Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier.versions of the robot. #### Publishes -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `_battery/battery_1_status_raw` [*sensor_msgs/BatteryState*]: first battery raw state. -- `_battery/battery_2_status_raw` [*sensor_msgs/BatteryState*]: second battery raw state. Published if second battery detected. -- `battery/battery_status` [*sensor_msgs/BatteryState*]: mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published. -- `battery/charging_status` [*panther_msgs/ChargingStatus*]: battery charging status. -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: battery diagnostic messages. +- `_battery/battery_1_status_raw` [*sensor_msgs/BatteryState*]: First battery raw state. +- `_battery/battery_2_status_raw` [*sensor_msgs/BatteryState*]: Second battery raw state. Published if second battery detected. +- `battery/battery_status` [*sensor_msgs/BatteryState*]: Mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published. +- `battery/charging_status` [*panther_msgs/ChargingStatus*]: Battery charging status. +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Battery diagnostic messages. -[//]: # (ROS_API_NODE_PUBLISHERS_END) +#### Subscribers -#### Subscribes - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `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. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) +- `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. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - - `~/adc/device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC nr 0 IIO device. Used with Panther version 1.2 and above. - `~/adc/device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC nr 1 IIO device. Used with Panther version 1.2 and above. -- `~/adc/ma_window_len/charge` [*int*, default: **10**]: window length of a moving average, used to smooth out battery charge readings. Used with Panther version 1.2 and above. -- `~/adc/ma_window_len/temp` [*int*, default: **10**]: window length of a moving average, used to smooth out battery temperature readings. Used with Panther version 1.2 and above. -- `~/battery_timeout` [*float*, default: **1.0**]: specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state. -- `~/ma_window_len/voltage` [*int*, default: **10**]: window length of a moving average, used to smooth out battery voltage readings. -- `~/ma_window_len/current` [*int*, default: **10**]: window length of a moving average, used to smooth out battery current readings. -- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: specifies timeout in seconds after which driver state messages will be considered old. Used with Panther version 1.06 and earlier. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) +- `~/adc/ma_window_len/charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings. Used with Panther version 1.2 and above. +- `~/adc/ma_window_len/temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings. Used with Panther version 1.2 and above. +- `~/battery_timeout` [*float*, default: **1.0**]: Specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state. +- `~/ma_window_len/voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings. +- `~/ma_window_len/current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings. +- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old. Used with Panther version 1.06 and earlier. diff --git a/panther_battery/include/panther_battery/adc_battery.hpp b/panther_battery/include/panther_battery/battery/adc_battery.hpp similarity index 94% rename from panther_battery/include/panther_battery/adc_battery.hpp rename to panther_battery/include/panther_battery/battery/adc_battery.hpp index 23e1ceb5f..6aebfa097 100644 --- a/panther_battery/include/panther_battery/adc_battery.hpp +++ b/panther_battery/include/panther_battery/battery/adc_battery.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_ADC_BATTERY_HPP_ -#define PANTHER_BATTERY_ADC_BATTERY_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_ +#define PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_ #include #include @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery.hpp" +#include "panther_battery/battery/battery.hpp" #include "panther_utils/moving_average.hpp" namespace panther_battery @@ -97,4 +97,4 @@ class ADCBattery : public Battery } // namespace panther_battery -#endif // PANTHER_BATTERY_ADC_BATTERY_HPP_ +#endif // PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_ diff --git a/panther_battery/include/panther_battery/battery.hpp b/panther_battery/include/panther_battery/battery/battery.hpp similarity index 85% rename from panther_battery/include/panther_battery/battery.hpp rename to panther_battery/include/panther_battery/battery/battery.hpp index ffd95ddea..eb067aa61 100644 --- a/panther_battery/include/panther_battery/battery.hpp +++ b/panther_battery/include/panther_battery/battery/battery.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_BATTERY_HPP_ -#define PANTHER_BATTERY_BATTERY_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_BATTERY_HPP_ +#define PANTHER_BATTERY_BATTERY_BATTERY_HPP_ #include #include @@ -57,7 +57,14 @@ class Battery float GetBatteryPercent(const float voltage) const { - return std::clamp((voltage - kVBatMin) / (kVBatFull - kVBatMin), 0.0f, 1.0f); + for (int i = 0; i < 4; i++) { + if (voltage > battery_approx_ranges[i]) { + return std::clamp( + (battery_approx_a_values[i] * voltage + battery_approx_b_values[i]) / 100, 0.0f, 1.0f); + } + } + return std::clamp( + (battery_approx_a_values[4] * voltage + battery_approx_b_values[4]) / 100, 0.0f, 1.0f); } void ResetBatteryMsgs(const rclcpp::Time & header_stamp) @@ -105,13 +112,15 @@ class Battery static constexpr float kBatDetectTresh = 3.03; static constexpr float kVBatFatalMin = 27.0; static constexpr float kVBatFatalMax = 43.0; - static constexpr float kVBatFull = 41.4; - static constexpr float kVBatMin = 32.0; static constexpr float kLowBatTemp = -10.0; static constexpr float kOverheatBatTemp = 45.0; static constexpr float kDesignedCapacity = 20.0; static constexpr std::string_view kLocation = "user_compartment"; + static constexpr float battery_approx_ranges[4] = {41.25, 37, 35, 33.7}; + static constexpr float battery_approx_a_values[5] = {1.733, 9.153, 19.8, 10.538, 0.989}; + static constexpr float battery_approx_b_values[5] = {27.214, -278.861, -672.6, -351.47, -29.67}; + std::string error_msg_; BatteryStateMsg battery_state_; BatteryStateMsg battery_state_raw_; @@ -120,4 +129,4 @@ class Battery } // namespace panther_battery -#endif // PANTHER_BATTERY_BATTERY_HPP_ +#endif // PANTHER_BATTERY_BATTERY_BATTERY_HPP_ diff --git a/panther_battery/include/panther_battery/roboteq_battery.hpp b/panther_battery/include/panther_battery/battery/roboteq_battery.hpp similarity index 91% rename from panther_battery/include/panther_battery/roboteq_battery.hpp rename to panther_battery/include/panther_battery/battery/roboteq_battery.hpp index 7daa3d6b7..a0019fc53 100644 --- a/panther_battery/include/panther_battery/roboteq_battery.hpp +++ b/panther_battery/include/panther_battery/battery/roboteq_battery.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_ROBOTEQ_BATTERY_HPP_ -#define PANTHER_BATTERY_ROBOTEQ_BATTERY_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ +#define PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ #include #include @@ -23,7 +23,7 @@ #include "panther_msgs/msg/driver_state.hpp" -#include "panther_battery/battery.hpp" +#include "panther_battery/battery/battery.hpp" #include "panther_utils/moving_average.hpp" namespace panther_battery @@ -76,4 +76,4 @@ class RoboteqBattery : public Battery } // namespace panther_battery -#endif // PANTHER_BATTERY_ROBOTEQ_BATTERY_HPP_ +#endif // PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ diff --git a/panther_battery/include/panther_battery/battery_node.hpp b/panther_battery/include/panther_battery/battery_driver_node.hpp similarity index 83% rename from panther_battery/include/panther_battery/battery_node.hpp rename to panther_battery/include/panther_battery/battery_driver_node.hpp index f431b5993..c13371c9d 100644 --- a/panther_battery/include/panther_battery/battery_node.hpp +++ b/panther_battery/include/panther_battery/battery_driver_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_BATTERY_NODE_HPP_ -#define PANTHER_BATTERY_BATTERY_NODE_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_ +#define PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_ #include #include @@ -24,18 +24,18 @@ #include "panther_msgs/msg/driver_state.hpp" #include "panther_battery/adc_data_reader.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" namespace panther_battery { using DriverStateMsg = panther_msgs::msg::DriverState; -class BatteryNode : public rclcpp::Node +class BatteryDriverNode : public rclcpp::Node { public: - BatteryNode( + BatteryDriverNode( const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: @@ -62,4 +62,4 @@ class BatteryNode : public rclcpp::Node } // namespace panther_battery -#endif // PANTHER_BATTERY_BATTERY_NODE_HPP_ +#endif // PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_ diff --git a/panther_battery/include/panther_battery/battery_publisher.hpp b/panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp similarity index 91% rename from panther_battery/include/panther_battery/battery_publisher.hpp rename to panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp index 30a110603..688f1899c 100644 --- a/panther_battery/include/panther_battery/battery_publisher.hpp +++ b/panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_HPP_ -#define PANTHER_BATTERY_BATTERY_PUBLISHER_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ +#define PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ #include @@ -73,4 +73,4 @@ class BatteryPublisher } // namespace panther_battery -#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_HPP_ +#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ diff --git a/panther_battery/include/panther_battery/dual_battery_publisher.hpp b/panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp similarity index 87% rename from panther_battery/include/panther_battery/dual_battery_publisher.hpp rename to panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp index 786952528..9890465fb 100644 --- a/panther_battery/include/panther_battery/dual_battery_publisher.hpp +++ b/panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_DUAL_BATTERY_PUBLISHER_HPP_ -#define PANTHER_BATTERY_DUAL_BATTERY_PUBLISHER_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ +#define PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ #include #include #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" namespace panther_battery { @@ -68,4 +68,4 @@ class DualBatteryPublisher : public BatteryPublisher } // namespace panther_battery -#endif // PANTHER_BATTERY_DUAL_BATTERY_PUBLISHER_HPP_ +#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ diff --git a/panther_battery/include/panther_battery/single_battery_publisher.hpp b/panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp similarity index 82% rename from panther_battery/include/panther_battery/single_battery_publisher.hpp rename to panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp index 957258d31..29be9bc5c 100644 --- a/panther_battery/include/panther_battery/single_battery_publisher.hpp +++ b/panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_SINGLE_BATTERY_PUBLISHER_HPP_ -#define PANTHER_BATTERY_SINGLE_BATTERY_PUBLISHER_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ +#define PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ #include #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" namespace panther_battery { @@ -53,4 +53,4 @@ class SingleBatteryPublisher : public BatteryPublisher } // namespace panther_battery -#endif // PANTHER_BATTERY_SINGLE_BATTERY_PUBLISHER_HPP_ +#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ diff --git a/panther_battery/launch/battery.launch.py b/panther_battery/launch/battery.launch.py index 8a3d7d708..3719d5872 100644 --- a/panther_battery/launch/battery.launch.py +++ b/panther_battery/launch/battery.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): battery_driver_node = Node( package="panther_battery", - executable="battery_node", + executable="battery_driver_node", name="battery_driver", parameters=[{"panther_version": panther_version}], namespace=namespace, diff --git a/panther_battery/package.xml b/panther_battery/package.xml index 29f20b152..8e67a1b9b 100644 --- a/panther_battery/package.xml +++ b/panther_battery/package.xml @@ -2,7 +2,7 @@ panther_battery - 2.1.0 + 2.1.1 Nodes monitoring the battery state of Husarion Panhter robot Husarion Apache License 2.0 diff --git a/panther_battery/src/adc_battery.cpp b/panther_battery/src/battery/adc_battery.cpp similarity index 99% rename from panther_battery/src/adc_battery.cpp rename to panther_battery/src/battery/adc_battery.cpp index 7bd76a762..968f48236 100644 --- a/panther_battery/src/adc_battery.cpp +++ b/panther_battery/src/battery/adc_battery.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/adc_battery.hpp" +#include "panther_battery/battery/adc_battery.hpp" #include #include diff --git a/panther_battery/src/roboteq_battery.cpp b/panther_battery/src/battery/roboteq_battery.cpp similarity index 98% rename from panther_battery/src/roboteq_battery.cpp rename to panther_battery/src/battery/roboteq_battery.cpp index e9097f8e8..6653208ec 100644 --- a/panther_battery/src/roboteq_battery.cpp +++ b/panther_battery/src/battery/roboteq_battery.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/roboteq_battery.hpp" +#include "panther_battery/battery/roboteq_battery.hpp" #include #include diff --git a/panther_battery/src/battery_node.cpp b/panther_battery/src/battery_driver_node.cpp similarity index 87% rename from panther_battery/src/battery_node.cpp rename to panther_battery/src/battery_driver_node.cpp index 3e9ff8d68..d1ff8acd2 100644 --- a/panther_battery/src/battery_node.cpp +++ b/panther_battery/src/battery_driver_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/battery_node.hpp" +#include "panther_battery/battery_driver_node.hpp" #include #include @@ -24,18 +24,19 @@ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" -#include "panther_battery/adc_battery.hpp" #include "panther_battery/adc_data_reader.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" -#include "panther_battery/dual_battery_publisher.hpp" -#include "panther_battery/roboteq_battery.hpp" -#include "panther_battery/single_battery_publisher.hpp" +#include "panther_battery/battery/adc_battery.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery/roboteq_battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" +#include "panther_battery/battery_publisher/dual_battery_publisher.hpp" +#include "panther_battery/battery_publisher/single_battery_publisher.hpp" namespace panther_battery { -BatteryNode::BatteryNode(const std::string & node_name, const rclcpp::NodeOptions & options) +BatteryDriverNode::BatteryDriverNode( + const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), diagnostic_updater_(std::make_shared(this)) { RCLCPP_INFO(this->get_logger(), "Constructing node."); @@ -46,14 +47,14 @@ BatteryNode::BatteryNode(const std::string & node_name, const rclcpp::NodeOption // Running at 10 Hz battery_pub_timer_ = this->create_wall_timer( - std::chrono::milliseconds(100), std::bind(&BatteryNode::BatteryPubTimerCB, this)); + std::chrono::milliseconds(100), std::bind(&BatteryDriverNode::BatteryPubTimerCB, this)); diagnostic_updater_->setHardwareID("Battery"); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } -void BatteryNode::Initialize() +void BatteryDriverNode::Initialize() { RCLCPP_INFO(this->get_logger(), "Initializing."); @@ -74,7 +75,7 @@ void BatteryNode::Initialize() RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void BatteryNode::InitializeWithADCBattery() +void BatteryDriverNode::InitializeWithADCBattery() { RCLCPP_DEBUG(this->get_logger(), "Initializing with ADC data."); @@ -127,7 +128,7 @@ void BatteryNode::InitializeWithADCBattery() RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data."); } -void BatteryNode::InitializeWithRoboteqBattery() +void BatteryDriverNode::InitializeWithRoboteqBattery() { RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data."); @@ -151,7 +152,7 @@ void BatteryNode::InitializeWithRoboteqBattery() RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data."); } -void BatteryNode::BatteryPubTimerCB() +void BatteryDriverNode::BatteryPubTimerCB() { if (!battery_publisher_) { Initialize(); diff --git a/panther_battery/src/battery_publisher.cpp b/panther_battery/src/battery_publisher/battery_publisher.cpp similarity index 98% rename from panther_battery/src/battery_publisher.cpp rename to panther_battery/src/battery_publisher/battery_publisher.cpp index a6a910ff6..e3ccb14b8 100644 --- a/panther_battery/src/battery_publisher.cpp +++ b/panther_battery/src/battery_publisher/battery_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" #include #include diff --git a/panther_battery/src/dual_battery_publisher.cpp b/panther_battery/src/battery_publisher/dual_battery_publisher.cpp similarity index 98% rename from panther_battery/src/dual_battery_publisher.cpp rename to panther_battery/src/battery_publisher/dual_battery_publisher.cpp index af2c95ed6..fba8198d2 100644 --- a/panther_battery/src/dual_battery_publisher.cpp +++ b/panther_battery/src/battery_publisher/dual_battery_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/dual_battery_publisher.hpp" +#include "panther_battery/battery_publisher/dual_battery_publisher.hpp" #include #include @@ -24,8 +24,8 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" #include "panther_utils/ros_utils.hpp" namespace panther_battery diff --git a/panther_battery/src/single_battery_publisher.cpp b/panther_battery/src/battery_publisher/single_battery_publisher.cpp similarity index 94% rename from panther_battery/src/single_battery_publisher.cpp rename to panther_battery/src/battery_publisher/single_battery_publisher.cpp index de136258a..4916e647c 100644 --- a/panther_battery/src/single_battery_publisher.cpp +++ b/panther_battery/src/battery_publisher/single_battery_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/single_battery_publisher.hpp" +#include "panther_battery/battery_publisher/single_battery_publisher.hpp" #include #include @@ -23,8 +23,8 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" namespace panther_battery { diff --git a/panther_battery/src/main.cpp b/panther_battery/src/main.cpp index f2b87ad71..b759fd33a 100644 --- a/panther_battery/src/main.cpp +++ b/panther_battery/src/main.cpp @@ -18,16 +18,16 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery_node.hpp" +#include "panther_battery/battery_driver_node.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto battery_node = std::make_shared("battery_driver"); + auto battery_driver_node = std::make_shared("battery_driver"); try { - rclcpp::spin(battery_node); + rclcpp::spin(battery_driver_node); } catch (const std::runtime_error & e) { std::cerr << "[battery_driver] Caught exception: " << e.what() << std::endl; } diff --git a/panther_battery/test/test_adc_battery.cpp b/panther_battery/test/battery/test_adc_battery.cpp similarity index 96% rename from panther_battery/test/test_adc_battery.cpp rename to panther_battery/test/battery/test_adc_battery.cpp index ca5e8d01e..986f7cdb1 100644 --- a/panther_battery/test/test_adc_battery.cpp +++ b/panther_battery/test/battery/test_adc_battery.cpp @@ -24,7 +24,7 @@ #include "panther_msgs/msg/charging_status.hpp" -#include "panther_battery/adc_battery.hpp" +#include "panther_battery/battery/adc_battery.hpp" #include "panther_utils/test/test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -157,8 +157,6 @@ TEST_F(TestADCBattery, BatteryMsgValues) const auto voltage_factor = 25.04255; const auto current_factor = 20.0; const auto charge_factor = 2.5; - const auto V_bat_full = 41.4; - const auto V_bat_min = 32.0; const float voltage_raw_1 = 1.5; const float current_raw_1 = 0.01; @@ -167,7 +165,7 @@ TEST_F(TestADCBattery, BatteryMsgValues) UpdateBattery(voltage_raw_1, current_raw_1, temp_raw_1, charge_raw_1, false); float expected_voltage = voltage_raw_1 * voltage_factor; - float expected_percentage = (expected_voltage - V_bat_min) / (V_bat_full - V_bat_min); + float expected_percentage = 0.64960694; float expected_temp = 28.875206; float expected_current = -(current_raw_1 * current_factor) + (charge_raw_1 * charge_factor); TestBatteryStateMsg( @@ -185,7 +183,7 @@ TEST_F(TestADCBattery, BatteryMsgValues) const float current_mean = (current_raw_1 + current_raw_2) / 2.0; const float charge_mean = (charge_raw_1 + charge_raw_2) / 2.0; expected_voltage = voltage_mean * voltage_factor; - expected_percentage = (expected_voltage - V_bat_min) / (V_bat_full - V_bat_min); + expected_percentage = 0.76421416; expected_temp = 30.306725; expected_current = -(current_mean * current_factor) + (charge_mean * charge_factor); TestBatteryStateMsg( @@ -195,7 +193,7 @@ TEST_F(TestADCBattery, BatteryMsgValues) // Raw battery message should depend only on last readings battery_state_ = battery_->GetBatteryMsgRaw(); expected_voltage = voltage_raw_2 * voltage_factor; - expected_percentage = (expected_voltage - V_bat_min) / (V_bat_full - V_bat_min); + expected_percentage = 0.87882143; expected_temp = 31.738245; expected_current = -(current_raw_2 * current_factor) + (charge_raw_2 * charge_factor); TestBatteryStateMsg( @@ -242,7 +240,7 @@ TEST_F(TestADCBattery, BatteryMsgHealthCold) TEST_F(TestADCBattery, BatteryMsgStatusFull) { - UpdateBattery(1.66, 0.01, 0.98, 0.5, true); + UpdateBattery(1.69, 0.01, 0.98, 0.5, true); EXPECT_FLOAT_EQ(1.0, battery_state_.percentage); EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_FULL, battery_state_.power_supply_status); diff --git a/panther_battery/test/test_battery.cpp b/panther_battery/test/battery/test_battery.cpp similarity index 95% rename from panther_battery/test/test_battery.cpp rename to panther_battery/test/battery/test_battery.cpp index 8ca8b090d..cdbdf3812 100644 --- a/panther_battery/test/test_battery.cpp +++ b/panther_battery/test/battery/test_battery.cpp @@ -24,7 +24,7 @@ #include "panther_msgs/msg/charging_status.hpp" -#include "panther_battery/battery.hpp" +#include "panther_battery/battery/battery.hpp" #include "panther_utils/test/test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -109,9 +109,9 @@ TEST_F(TestBattery, GetErrorMsg) TEST_F(TestBattery, GetBatteryPercent) { EXPECT_FLOAT_EQ(0.0, battery_->GetBatteryPercent(30.0f)); - EXPECT_FLOAT_EQ(0.0, battery_->GetBatteryPercent(32.0f)); - EXPECT_FLOAT_EQ(0.638297872, battery_->GetBatteryPercent(38.0f)); - EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(41.4f)); + EXPECT_FLOAT_EQ(0.019780006, battery_->GetBatteryPercent(32.0f)); + EXPECT_FLOAT_EQ(0.68953001, battery_->GetBatteryPercent(38.0f)); + EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(42.0f)); EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(45.0f)); } diff --git a/panther_battery/test/test_roboteq_battery.cpp b/panther_battery/test/battery/test_roboteq_battery.cpp similarity index 95% rename from panther_battery/test/test_roboteq_battery.cpp rename to panther_battery/test/battery/test_roboteq_battery.cpp index 3b1866546..b7059e07c 100644 --- a/panther_battery/test/test_roboteq_battery.cpp +++ b/panther_battery/test/battery/test_roboteq_battery.cpp @@ -26,7 +26,7 @@ #include "panther_msgs/msg/driver_state.hpp" -#include "panther_battery/roboteq_battery.hpp" +#include "panther_battery/battery/roboteq_battery.hpp" #include "panther_utils/test/test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -155,15 +155,12 @@ TEST_F(TestRoboteqBattery, BatteryMsgUnknown) TEST_F(TestRoboteqBattery, BatteryMsgValues) { - const float V_bat_full = 41.4; - const float V_bat_min = 32.0; - const float voltage_1 = 35.0; const float current_1 = 0.1; UpdateBattery(voltage_1, current_1); float expected_voltage = voltage_1; - float expected_percentage = (expected_voltage - V_bat_min) / (V_bat_full - V_bat_min); + float expected_percentage = 0.17360015; float expected_current = current_1 * 2.0; TestBatteryStateMsg( expected_voltage, expected_current, expected_percentage, @@ -174,7 +171,7 @@ TEST_F(TestRoboteqBattery, BatteryMsgValues) UpdateBattery(voltage_2, current_2); expected_voltage = (voltage_1 + voltage_2) / 2.0; - expected_percentage = (expected_voltage - V_bat_min) / (V_bat_full - V_bat_min); + expected_percentage = 0.40200013; expected_current = (current_1 * 2.0 + current_2 * 2.0) / 2.0; TestBatteryStateMsg( expected_voltage, expected_current, expected_percentage, @@ -183,7 +180,7 @@ TEST_F(TestRoboteqBattery, BatteryMsgValues) // Check raw battery msg battery_state_ = battery_->GetBatteryMsgRaw(); expected_voltage = voltage_2; - expected_percentage = (expected_voltage - V_bat_min) / (V_bat_full - V_bat_min); + expected_percentage = 0.60000002; expected_current = current_2 * 2.0; TestBatteryStateMsg( expected_voltage, expected_current, expected_percentage, diff --git a/panther_battery/test/test_battery_publisher.cpp b/panther_battery/test/battery_publisher/test_battery_publisher.cpp similarity index 98% rename from panther_battery/test/test_battery_publisher.cpp rename to panther_battery/test/battery_publisher/test_battery_publisher.cpp index 45f74a61a..4ed567b16 100644 --- a/panther_battery/test/test_battery_publisher.cpp +++ b/panther_battery/test/battery_publisher/test_battery_publisher.cpp @@ -24,7 +24,7 @@ #include "panther_msgs/msg/io_state.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; using IOStateMsg = panther_msgs::msg::IOState; diff --git a/panther_battery/test/test_dual_battery_publisher.cpp b/panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp similarity index 98% rename from panther_battery/test/test_dual_battery_publisher.cpp rename to panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp index 529fbddf0..0740b7fa9 100644 --- a/panther_battery/test/test_dual_battery_publisher.cpp +++ b/panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp @@ -24,9 +24,9 @@ #include "panther_msgs/msg/charging_status.hpp" -#include "panther_battery/adc_battery.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/dual_battery_publisher.hpp" +#include "panther_battery/battery/adc_battery.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/dual_battery_publisher.hpp" #include "panther_utils/test/ros_test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; diff --git a/panther_battery/test/test_single_battery_publisher.cpp b/panther_battery/test/battery_publisher/test_single_battery_publisher.cpp similarity index 94% rename from panther_battery/test/test_single_battery_publisher.cpp rename to panther_battery/test/battery_publisher/test_single_battery_publisher.cpp index 345f85086..f7a04c3fa 100644 --- a/panther_battery/test/test_single_battery_publisher.cpp +++ b/panther_battery/test/battery_publisher/test_single_battery_publisher.cpp @@ -21,9 +21,9 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_battery/adc_battery.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/single_battery_publisher.hpp" +#include "panther_battery/battery/adc_battery.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/single_battery_publisher.hpp" #include "panther_utils/test/ros_test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; diff --git a/panther_battery/test/test_battery_node_dual_bat.cpp b/panther_battery/test/test_battery_driver_node_adc_dual.cpp similarity index 82% rename from panther_battery/test/test_battery_node_dual_bat.cpp rename to panther_battery/test/test_battery_driver_node_adc_dual.cpp index 653f0f65c..414fce6ba 100644 --- a/panther_battery/test/test_battery_node_dual_bat.cpp +++ b/panther_battery/test/test_battery_driver_node_adc_dual.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "include/test_battery_node.hpp" +#include "utils/test_battery_driver_node.hpp" #include @@ -22,24 +22,24 @@ #include "panther_utils/test/ros_test_utils.hpp" -class TestBatteryNodeDualBattery : public TestBatteryNode +class TestBatteryNodeADCDual : public TestBatteryNode { public: - TestBatteryNodeDualBattery() : TestBatteryNode(1.2, true) {} + TestBatteryNodeADCDual() : TestBatteryNode(1.2, true) {} }; -TEST_F(TestBatteryNodeDualBattery, BatteryValues) +TEST_F(TestBatteryNodeADCDual, BatteryValues) { ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // This is done to check if channels of ADC readers were assigned correctly, not to verify // calculations. If any test performing calculations fails this test will most likely fail too. EXPECT_FLOAT_EQ(35.05957, battery_state_->voltage); EXPECT_FLOAT_EQ(2.02, battery_state_->current); EXPECT_FLOAT_EQ(26.094543, battery_state_->temperature); - EXPECT_FLOAT_EQ(0.32548615, battery_state_->percentage); - EXPECT_FLOAT_EQ(13.019446, battery_state_->charge); + EXPECT_FLOAT_EQ(0.21579468, battery_state_->percentage); + EXPECT_FLOAT_EQ(8.6317873, battery_state_->charge); // If both batteries have the same reading values they should have equal values EXPECT_FLOAT_EQ(battery_1_state_->voltage, battery_2_state_->voltage); @@ -52,7 +52,7 @@ TEST_F(TestBatteryNodeDualBattery, BatteryValues) // stops matching WriteNumberToFile(1600, std::filesystem::path(device1_path_ / "in_voltage3_raw")); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_2_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000))); EXPECT_FALSE( fabs(battery_1_state_->voltage - battery_2_state_->voltage) < std::numeric_limits::epsilon()); @@ -68,7 +68,7 @@ TEST_F(TestBatteryNodeDualBattery, BatteryValues) WriteNumberToFile(900, std::filesystem::path(device1_path_ / "in_voltage1_raw")); WriteNumberToFile(100, std::filesystem::path(device0_path_ / "in_voltage2_raw")); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_2_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000))); EXPECT_FALSE( fabs(battery_1_state_->current - battery_2_state_->current) < std::numeric_limits::epsilon()); @@ -76,16 +76,16 @@ TEST_F(TestBatteryNodeDualBattery, BatteryValues) WriteNumberToFile(1000, std::filesystem::path(device0_path_ / "in_voltage0_raw")); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_2_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000))); EXPECT_FALSE( fabs(battery_1_state_->temperature - battery_2_state_->temperature) < std::numeric_limits::epsilon()); } -TEST_F(TestBatteryNodeDualBattery, BatteryTimeout) +TEST_F(TestBatteryNodeADCDual, BatteryTimeout) { ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg should have some values EXPECT_FALSE(std::isnan(battery_state_->voltage)); @@ -99,7 +99,7 @@ TEST_F(TestBatteryNodeDualBattery, BatteryTimeout) std::filesystem::remove(std::filesystem::path(device1_path_ / "in_voltage2_raw")); std::this_thread::sleep_for(std::chrono::milliseconds(2500)); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); @@ -111,18 +111,18 @@ TEST_F(TestBatteryNodeDualBattery, BatteryTimeout) EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health); } -TEST_F(TestBatteryNodeDualBattery, BatteryCharging) +TEST_F(TestBatteryNodeADCDual, BatteryCharging) { // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Publish charger connected state IOStateMsg io_state; io_state.charger_connected = true; io_state_pub_->publish(io_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); EXPECT_NE(battery_state_->power_supply_status, BatteryStateMsg::POWER_SUPPLY_STATUS_DISCHARGING); } diff --git a/panther_battery/test/test_battery_node.cpp b/panther_battery/test/test_battery_driver_node_adc_single.cpp similarity index 79% rename from panther_battery/test/test_battery_node.cpp rename to panther_battery/test/test_battery_driver_node_adc_single.cpp index 3d9fdaac2..6ddd48b38 100644 --- a/panther_battery/test/test_battery_node.cpp +++ b/panther_battery/test/test_battery_driver_node_adc_single.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "include/test_battery_node.hpp" +#include "utils/test_battery_driver_node.hpp" #include @@ -22,7 +22,13 @@ #include "panther_utils/test/ros_test_utils.hpp" -TEST_F(TestBatteryNode, BatteryValues) +class TestBatteryNodeADCSingle : public TestBatteryNode +{ +public: + TestBatteryNodeADCSingle() : TestBatteryNode(1.2, false) {} +}; + +TEST_F(TestBatteryNodeADCSingle, BatteryValues) { // Change some values for ADC channels used by second battery. // As this is test for single battery this should not affect published battery state. @@ -30,15 +36,15 @@ TEST_F(TestBatteryNode, BatteryValues) WriteNumberToFile(100, std::filesystem::path(device0_path_ / "in_voltage2_raw")); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // This is done to check if channels of ADC readers were assigned correctly, not to verify // calculations. If any test performing calculations fails this test will most likely fail too. EXPECT_FLOAT_EQ(35.05957, battery_state_->voltage); EXPECT_FLOAT_EQ(2.01, battery_state_->current); EXPECT_FLOAT_EQ(26.094543, battery_state_->temperature); - EXPECT_FLOAT_EQ(0.32548615, battery_state_->percentage); - EXPECT_FLOAT_EQ(6.5097232, battery_state_->charge); + EXPECT_FLOAT_EQ(0.21579468, battery_state_->percentage); + EXPECT_FLOAT_EQ(4.3158937, battery_state_->charge); // For single battery if readings stay the same values of battery_1 and battery should be the same EXPECT_FLOAT_EQ(battery_1_state_->voltage, battery_state_->voltage); @@ -48,10 +54,10 @@ TEST_F(TestBatteryNode, BatteryValues) EXPECT_FLOAT_EQ(battery_1_state_->charge, battery_state_->charge); } -TEST_F(TestBatteryNode, BatteryTimeout) +TEST_F(TestBatteryNodeADCSingle, BatteryTimeout) { ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg should have some values EXPECT_FALSE(std::isnan(battery_state_->voltage)); @@ -65,7 +71,7 @@ TEST_F(TestBatteryNode, BatteryTimeout) std::filesystem::remove(std::filesystem::path(device1_path_ / "in_voltage2_raw")); std::this_thread::sleep_for(std::chrono::milliseconds(2500)); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); @@ -77,41 +83,41 @@ TEST_F(TestBatteryNode, BatteryTimeout) EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health); } -TEST_F(TestBatteryNode, BatteryCharging) +TEST_F(TestBatteryNodeADCSingle, BatteryCharging) { // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Publish charger connected state IOStateMsg io_state; io_state.charger_connected = true; io_state_pub_->publish(io_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); EXPECT_NE(battery_state_->power_supply_status, BatteryStateMsg::POWER_SUPPLY_STATUS_DISCHARGING); } -TEST_F(TestBatteryNode, RoboteqInitOnADCFail) +TEST_F(TestBatteryNodeADCSingle, RoboteqInitOnADCFail) { // Remove ADC device std::filesystem::remove_all(device0_path_); // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state status should be UNKNOWN EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN, battery_state_->power_supply_status); // Publish driver state that should update the battery message DriverStateMsg driver_state; - driver_state.header.stamp = battery_node_->get_clock()->now(); + driver_state.header.stamp = battery_driver_node_->get_clock()->now(); driver_state_pub_->publish(driver_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state status should be different than UNKNOWN EXPECT_NE(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN, battery_state_->power_supply_status); diff --git a/panther_battery/test/test_battery_node_roboteq.cpp b/panther_battery/test/test_battery_driver_node_roboteq.cpp similarity index 88% rename from panther_battery/test/test_battery_node_roboteq.cpp rename to panther_battery/test/test_battery_driver_node_roboteq.cpp index 4240792ad..c4cf8f9c5 100644 --- a/panther_battery/test/test_battery_node_roboteq.cpp +++ b/panther_battery/test/test_battery_driver_node_roboteq.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "include/test_battery_node.hpp" +#include "utils/test_battery_driver_node.hpp" #include #include @@ -34,7 +34,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues) { // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); @@ -45,7 +45,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues) EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health); DriverStateMsg driver_state; - driver_state.header.stamp = battery_node_->get_clock()->now(); + driver_state.header.stamp = battery_driver_node_->get_clock()->now(); driver_state.front.voltage = 35.0f; driver_state.rear.voltage = 35.0f; driver_state.front.current = 0.1f; @@ -53,7 +53,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues) driver_state_pub_->publish(driver_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // This is done to check if values were read correctly, not to verify calculations. // If any test performing calculations fails this test will most likely fail too. @@ -71,7 +71,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) { // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); @@ -83,7 +83,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) // Publish some values DriverStateMsg driver_state; - driver_state.header.stamp = battery_node_->get_clock()->now(); + driver_state.header.stamp = battery_driver_node_->get_clock()->now(); driver_state.front.voltage = 35.0f; driver_state.rear.voltage = 35.0f; driver_state.front.current = 0.1f; @@ -91,7 +91,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) driver_state_pub_->publish(driver_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg should have some values EXPECT_FALSE(std::isnan(battery_state_->voltage)); @@ -102,7 +102,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) // Wait for timeout std::this_thread::sleep_for(std::chrono::milliseconds(1500)); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); diff --git a/panther_battery/test/include/test_battery_node.hpp b/panther_battery/test/utils/test_battery_driver_node.hpp similarity index 87% rename from panther_battery/test/include/test_battery_node.hpp rename to panther_battery/test/utils/test_battery_driver_node.hpp index a8a89305e..9b2385b2b 100644 --- a/panther_battery/test/include/test_battery_node.hpp +++ b/panther_battery/test/utils/test_battery_driver_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_TEST_BATTERY_NODE_ -#define PANTHER_BATTERY_TEST_BATTERY_NODE_ +#ifndef PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ +#define PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ #include #include @@ -31,7 +31,7 @@ #include "panther_msgs/msg/driver_state.hpp" #include "panther_msgs/msg/io_state.hpp" -#include "panther_battery/battery_node.hpp" +#include "panther_battery/battery_driver_node.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; using DriverStateMsg = panther_msgs::msg::DriverState; @@ -55,7 +55,7 @@ class TestBatteryNode : public testing::Test BatteryStateMsg::SharedPtr battery_state_; BatteryStateMsg::SharedPtr battery_1_state_; BatteryStateMsg::SharedPtr battery_2_state_; - std::shared_ptr battery_node_; + std::shared_ptr battery_driver_node_; rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr battery_1_sub_; rclcpp::Subscription::SharedPtr battery_2_sub_; @@ -104,21 +104,22 @@ TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_ba rclcpp::NodeOptions options; options.parameter_overrides(params); - battery_node_ = std::make_shared("battery_driver", options); + battery_driver_node_ = std::make_shared( + "battery_driver", options); - battery_sub_ = battery_node_->create_subscription( + battery_sub_ = battery_driver_node_->create_subscription( "battery/battery_status", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_state_ = msg; }); - battery_1_sub_ = battery_node_->create_subscription( + battery_1_sub_ = battery_driver_node_->create_subscription( "_battery/battery_1_status_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; }); - battery_2_sub_ = battery_node_->create_subscription( + battery_2_sub_ = battery_driver_node_->create_subscription( "_battery/battery_2_status_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_2_state_ = msg; }); - io_state_pub_ = battery_node_->create_publisher( + io_state_pub_ = battery_driver_node_->create_publisher( "hardware/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - driver_state_pub_ = battery_node_->create_publisher( + driver_state_pub_ = battery_driver_node_->create_publisher( "hardware/motor_controllers_state", 10); } @@ -145,4 +146,4 @@ void TestBatteryNode::WriteNumberToFile(const T number, const std::string & file } } -#endif // PANTHER_BATTERY_TEST_BATTERY_NODE_ +#endif // PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ diff --git a/panther_bringup/CHANGELOG.rst b/panther_bringup/CHANGELOG.rst index ee2cfca9b..da3524f36 100644 --- a/panther_bringup/CHANGELOG.rst +++ b/panther_bringup/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package panther_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.1 (2024-09-05) +------------------ +* Merge branch 'ros2-devel' into ros2-ns-refactor +* Merge branch 'ros2-devel' of https://github.com/husarion/panther_ros into ros2-add-nmea-gps +* Merge remote-tracking branch 'origin/ros2-devel' into ros2-testing-poc +* Merge pull request `#386 `_ from husarion/ros2-unify-filenames +* Ros2 estop sim gui (`#384 `_) +* Rename config and launch file in manager package +* Merge branch 'ros2-devel' into ros2-ns-refactor +* Update after changes in panther_diagnostics +* New format of documentation (`#369 `_) +* Contributors: KmakD, Paweł Irzyk, pawelirh, rafal-gorecki + 2.1.0 (2024-08-02) ------------------ * Merge pull request `#362 `_ from husarion/ros2-api-reorganization diff --git a/panther_bringup/CMakeLists.txt b/panther_bringup/CMakeLists.txt index bd2e92f43..793057508 100644 --- a/panther_bringup/CMakeLists.txt +++ b/panther_bringup/CMakeLists.txt @@ -3,6 +3,6 @@ project(panther_bringup) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_bringup/README.md b/panther_bringup/README.md index dcc85e4ef..b12b4218d 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -1,162 +1,9 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_bringup -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) - -## Default Nodes Launched - -- `battery_driver` [*[panther_battery/battery_node](https://github.com/husarion/panther_ros/panther_battery/src/main.cpp)*]: node responsible for monitoring and publishing the internal Battery state of the Husarion Panther robot. For more information, refer to [panther_battery](https://github.com/husarion/panther_ros/panther_battery/README.md). -- `ekf_filter` [*[robot_localization/ekf_node](https://github.com/cra-ros-pkg/robot_localization/blob/humble-devel/src/ekf_node.cpp)*]: Extended Kalman Filter node for more accurate odometry. For more information, refer to [robot_localization](https://github.com/cra-ros-pkg/robot_localization/tree/noetic-devel). The default configuration is stored in [ekf_config.yaml](https://github.com/husarion/panther_ros/panther_gazebo/config/ekf_config.yaml). -- `imu_container` [*[phidgets_spatial/phidgets::SpatialRosI](https://github.com/ros-drivers/phidgets_drivers/blob/humble/phidgets_spatial/src/spatial_ros_i.cpp)*, *[imu_filter_madgwick/ImuFilterMadgwickRos](https://github.com/CCNYRoboticsLab/imu_tools/blob/humble/imu_filter_madgwick/src/imu_filter_node.cpp)*]: container responsible for running Phidget Spatial IMU ROS driver, filtering and fusing the IMU data. It composes the `phidgets_spatial_node` and `imu_filter_node`. - -## Bringup Launch Arguments - -- `battery_config_path` [*string*, default: **None**]: path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. -- `controller_config_path` [*string*, default: **panther_controller/config/_controller.yaml**]: path to controller configuration file. A path to custom configuration can be specified here. -- `disable_manager` [*bool*, default: **false**]: Enable or disable manager_bt_node. -- `ekf_config_path` [*string*, default: **panther_bringup/config/ekf.yaml**]: path to the EKF configuration file. -- `led_config_file` [*string*, default: **panther_lights/config/led_config.yaml**]: path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. -- `namespace` [*string*, default: **None**] Add namespace to all launched nodes. -- `publish_robot_state` [*bool*, default: **true**]: whether to publish the default Panther robot description. -- `shutdown_hosts_config_path` = [*string*, default: **panther_bringup/config/shutdown_hosts.yaml**]: Path to file with list of hosts to request shutdown. -- `use_ekf` [*bool*, default: **true**]: enable or disable Extended Kalman Filter. -- `use_sim` [*bool*, default: **false**]: whether simulation is used. -- `user_led_animations_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. -- `wheel_config_path` [*string*, default: **$(find panther_description)/config/.yaml**]: path to YAML file with wheel specification. Arguments become required if `wheel_type` is set to **custom**. -- `wheel_type` [*string*, default: **WH01**]: type of wheel, possible are: **WH01** - off-road, **WH02** - mecanum, **WH04** - small pneumatic, and **custom** - custom wheel types (requires setting `wheel_config_path` argument accordingly). -- `components_config_path` [*string*, default: **None**]: Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). Example of configuration you can find [config/components.yaml](config/components.yaml) - -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - -## External ROS Nodes - -[//]: # (ROS_API_PACKAGE_NAME_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### ekf_filter - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type: *[robot_localization/ekf_node](https://github.com/cra-ros-pkg/robot_localization/blob/humble-devel/src/ekf_node.cpp)*. - -Extended Kalman Filter node for more accurate odometry. For more information, refer to [robot_localization](https://github.com/cra-ros-pkg/robot_localization/tree/humble-devel). The default configuration is stored in `panther_bringup/config/ekf.yaml`. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Subscribers - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `~/odometry/wheels` [*nav_msgs/Odometry*]: robot odometry calculated from wheels. -- `~/imu/data` [*sensor_msgs/Imu*]: filtered IMU data. -- `/tf` [*tf2_msgs/TFMessage*]: transforms of robot system. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `~/odometry/filtered` [*nav_msgs/Odometry*]: provides filtered odometry information. This topic contains a fused and enhanced estimate of the robot's pose and velocity, incorporating data from various sensors and correcting for any errors in the estimated state. -- `/tf` [*tf2_msgs/TFMessage*]: publishes `odom` to `base_link` transform. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) - -#### Service Servers - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) - -- `localization/set_pose` [*robot_localization/SetPose*]: by issuing a *geometry_msgs/PoseWithCovarianceStamped* message to the set_pose topic, users can manually set the state of the filter. This is useful for resetting the filter during testing and allows for interaction with RViz. Alternatively, the state estimation nodes advertise a SetPose service, whose type is *robot_localization/SetPose*. - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### imu_filter_node - -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type: *[imu_filter_madgwick/imu_filter_node](https://github.com/CCNYRoboticsLab/imu_tools/blob/humble/imu_filter_madgwick/src/imu_filter_node.cpp)*. - -Node responsible for filtering and fusing raw data from the IMU. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Subscribers - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `~/imu/data_raw` [*sensor_msgs/Imu*]: the raw accelerometer and gyroscope data. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `~/imu/data` [*sensor_msgs/Imu*]: the fused IMU messages, containing the orientation. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### phidgets_spatial_node - -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type: *[phidgets_spatial/spatial_ros_i.cpp](https://github.com/ros-drivers/phidgets_drivers/blob/humble/phidgets_spatial/src/spatial_ros_i.cpp)*. - -The ROS driver for Phidgets Spatial. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `~/imu/data_raw` [*sensor_msgs/Imu*]: the raw accelerometer and gyroscope data. -- `~/imu/is_calibrated` [*std_msgs/Bool*]: whether the gyroscope has been calibrated. This will be done automatically at startup time but can also be re-done at any time by calling the `imu/calibrate` service. -- `~/imu/mag` [*sensor_msgs/MagneticField*]: the raw magnetometer data. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) - -#### Service Servers - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) - -- `~/imu/calibrate` [*std_srvs/Empty*]: run calibration on the gyroscope. +## Launch Files -[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) -[//]: # (ROS_API_NODE_END) +This package contains: -[//]: # (ROS_API_PACKAGE_END) +- `bringup.launch.py`: Responsible for activating whole robot system. diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index 8e1b3c12b..96fc04453 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): "disable_manager", default_value="False", description="Enable or disable manager_bt_node.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) namespace = LaunchConfiguration("namespace") @@ -48,7 +48,7 @@ def generate_launch_description(): "use_ekf", default_value="True", description="Enable or disable EKF.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) serial_no = EnvironmentVariable(name="PANTHER_SERIAL_NO", default_value="----") @@ -64,13 +64,13 @@ def generate_launch_description(): launch_arguments={"namespace": namespace}.items(), ) - system_status_launch = IncludeLaunchDescription( + system_monitor_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [ FindPackageShare("panther_diagnostics"), "launch", - "system_status.launch.py", + "system_monitor.launch.py", ] ), ), @@ -108,7 +108,7 @@ def generate_launch_description(): manager_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_manager"), "launch", "manager_bt.launch.py"] + [FindPackageShare("panther_manager"), "launch", "manager.launch.py"] ) ), condition=UnlessCondition(disable_manager), @@ -131,7 +131,7 @@ def generate_launch_description(): declare_use_ekf_arg, welcome_msg, controller_launch, - system_status_launch, + system_monitor_launch, delayed_action, ] diff --git a/panther_bringup/package.xml b/panther_bringup/package.xml index fc9913754..a5fe44b42 100644 --- a/panther_bringup/package.xml +++ b/panther_bringup/package.xml @@ -2,7 +2,7 @@ panther_bringup - 2.1.0 + 2.1.1 Default launch files and configuration used to start Husarion Panther robot Husarion Apache License 2.0 diff --git a/panther_controller/CHANGELOG.rst b/panther_controller/CHANGELOG.rst index 78ef1a956..8ab5b60ee 100644 --- a/panther_controller/CHANGELOG.rst +++ b/panther_controller/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package panther_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.1 (2024-09-05) +------------------ +* Merge pull request `#403 `_ from husarion/ros2-control-ns-fix +* Remove deprecated --namespace arg +* Merge remote-tracking branch 'origin/ros2-devel' into ros2-testing-poc +* Ros2 estop sim gui (`#384 `_) +* Merge branch 'ros2-devel' into ros2-ns-refactor +* unify CMakeLists.txt files (`#381 `_) +* unify CMakeLists.txt files +* New format of documentation (`#369 `_) +* Contributors: Dawid, Dawid Kmak, pawelirh, rafal-gorecki + 2.1.0 (2024-08-02) ------------------ * Fix imu tf frame (`#373 `_) diff --git a/panther_controller/CMakeLists.txt b/panther_controller/CMakeLists.txt index b71172368..c6489681f 100644 --- a/panther_controller/CMakeLists.txt +++ b/panther_controller/CMakeLists.txt @@ -3,6 +3,6 @@ project(panther_controller) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_controller/CONFIGURATION.md b/panther_controller/CONFIGURATION.md new file mode 100644 index 000000000..f541b8727 --- /dev/null +++ b/panther_controller/CONFIGURATION.md @@ -0,0 +1,9 @@ +# panther_controller + +## Changing Velocity Smoothing Parameters + +The Panther by default uses [diff_drive_controller](https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html) from [ros2 control](https://control.ros.org/master/index.html) or [mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller). This controller can be customized, among others: by modifying the robot's dynamic parameters (e.g. smooth the speed or limit the robot's speed and acceleration). Its parameters can be found in the [panther_controller](https://github.com/husarion/panther_ros/tree/ros2-devel/panther_controller/config). By default, these values correspond to the upper limits of the robot's velocities and accelerations. + +## Changing Wheel Type + +Changing wheel types is possible and can be done for both the real robot and the simulation. By default, three types of wheels are supported using the launch argument `wheel_type`. If you want to use custom wheels, all you need to do is point to the new wheel and controller configuration files using the `wheel_config_path` and `controller_config_path` parameters. These files should be based on the default files, i.e. [WH01_controller.yaml](./config/WH01_controller.yaml) and [WH01.yaml](../panther_description/config/WH01.yaml). diff --git a/panther_controller/README.md b/panther_controller/README.md index dc373312e..ac600dc72 100644 --- a/panther_controller/README.md +++ b/panther_controller/README.md @@ -1,122 +1,13 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_controller -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) - -## Default Nodes Launched - -- `ros2_control_node` [*[controller_manager/controller_manager](https://github.com/ros-controls/ros2_control/blob/master/controller_manager)*]: Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. For more information, refer to [controller_manager](https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html). This node manages the following controllers: - - `imu_broadcaster` [*[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)*]: The broadcaster to publish readings of IMU sensors. - - `joint_state_broadcaster` [*[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)*]: The broadcaster reads all state interfaces and reports them on specific topics. - - `drive_controller` [*[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller)* ]: Controller which manages mobile robots with a differential drive. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. -- `robot_state_publisher` [*[robot_state_publisher/robot_state_publisher](https://github.com/ros/robot_state_publisher)*]: The Robot State Publisher broadcasts a robot's state to tf2 using a provided URDF model and joint states. It updates the model and broadcasts poses for fixed and movable joints to tf2 topics. - -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - -## External ROS Nodes - -[//]: # (ROS_API_PACKAGE_NAME_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### imu_broadcaster - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type:*[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)*. - -The broadcaster to publish readings of IMU sensors. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `imu/data` [*sensor_msgs/msg/Imu*]: data from IMU sensor. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### joint_state_broadcaster - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type:*[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)*. - -The broadcaster reads all state interfaces and reports them on specific topics. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `dynamic_joint_states` [*control_msgs/msg/DynamicJointState*] - provides information about the state of various movable joints in a robotic system. -- `joint_states` [*sensor_msgs/msg/JointState*] - provides information about the state of various joints in a robotic system. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### drive_controller - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type:*[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller)*. - -Controller which manages mobile robots with a differential drive. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Subscribers - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `cmd_vel` [*geometry_msgs/msg/Twist*]: command linear and angular velocity values. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) +## Launch Files -- `/tf` [*tf2_msgs/msg/TFMessage*]: tf tree. Published only if `enable_odom_tf=true` -- `odometry/wheels` [*nav_msgs/msg/Odometry*]: odometry data from wheel encoders. +- `controller.launch.py`: Establishes communication with the hardware by loading the robot's URDF with plugins and configures the controllers to exchange information between the engine driver and the IMU. -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) +## Configuration Files -[//]: # (ROS_API_PACKAGE_END) +- [`WH01_controller.yaml`](./config/WH01_controller.yaml): Configures `imu_broadcaster`, `joint_state_broadcaster` and `drive_controller` controllers for default WH01 wheels. +- [`WH02_controller.yaml`](./config/WH02_controller.yaml): Configures `imu_broadcaster`, `joint_state_broadcaster` and `drive_controller` controllers for mecanum WH02 wheels. +- [`WH04_controller.yaml`](./config/WH04_controller.yaml): Configures `imu_broadcaster`, `joint_state_broadcaster` and `drive_controller` controllers for small pneumatic WH04 wheels. diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py index 8a34ef7a7..0ad64e583 100644 --- a/panther_controller/launch/controller.launch.py +++ b/panther_controller/launch/controller.launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): ), description=( "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/_controller.yaml'. You can also specify" + " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify" " the path to your custom controller configuration file here. " ), ) @@ -93,7 +93,7 @@ def generate_launch_description(): "Whether to launch the robot_state_publisher node." "When set to False, users should publish their own robot description." ), - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) wheel_config_path = LaunchConfiguration("wheel_config_path") @@ -101,7 +101,7 @@ def generate_launch_description(): "use_sim", default_value="False", description="Whether simulation is used", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) declare_wheel_config_path_arg = DeclareLaunchArgument( @@ -115,7 +115,7 @@ def generate_launch_description(): ), description=( "Path to wheel configuration file. By default, it is located in " - "'panther_description/config/.yaml'. You can also specify the path " + "'panther_description/config/{wheel_type}.yaml'. You can also specify the path " "to your custom wheel configuration file here. " ), ) @@ -219,8 +219,6 @@ def generate_launch_description(): "controller_manager", "--controller-manager-timeout", "10", - "--namespace", - namespace, ], namespace=namespace, emulate_tty=True, @@ -235,8 +233,6 @@ def generate_launch_description(): "controller_manager", "--controller-manager-timeout", "10", - "--namespace", - namespace, ], namespace=namespace, emulate_tty=True, @@ -259,8 +255,6 @@ def generate_launch_description(): "controller_manager", "--controller-manager-timeout", "10", - "--namespace", - namespace, ], namespace=namespace, emulate_tty=True, diff --git a/panther_controller/package.xml b/panther_controller/package.xml index e1b7ea0ba..067db90ce 100644 --- a/panther_controller/package.xml +++ b/panther_controller/package.xml @@ -2,7 +2,7 @@ panther_controller - 2.1.0 + 2.1.1 ros2 controllers configuration for Panther Husarion Apache License 2.0 diff --git a/panther_description/CHANGELOG.rst b/panther_description/CHANGELOG.rst index ef1ac406f..07a9fce1e 100644 --- a/panther_description/CHANGELOG.rst +++ b/panther_description/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package panther_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.1 (2024-09-05) +------------------ +* LEDStrip plugin to Gazebo (`#391 `_) +* Merge pull request `#374 `_ from husarion/ros2-ns-refactor +* Merge branch 'ros2-devel' into ros2-ns-refactor +* Merge branch 'ros2-devel' of https://github.com/husarion/panther_ros into ros2-add-nmea-gps +* Merge pull request `#383 `_ from husarion/ros-sim-estop +* Merge remote-tracking branch 'origin/ros2-devel' into ros2-testing-poc +* Rename PantherSystem -> GzPantherSystem +* Typos in Readme + estop publish on service call +* Ros2 estop sim gui (`#384 `_) +* Merge branch 'ros2-devel' into ros2-ns-refactor +* unify CMakeLists.txt files (`#381 `_) +* Add remapping +* unify CMakeLists.txt files +* Add EStop to Gazebo +* New format of documentation (`#369 `_) +* Namespace refactor +* Contributors: Dawid, Dawid Kmak, KmakD, Paweł Irzyk, pawelirh, rafal-gorecki + 2.1.0 (2024-08-02) ------------------ * Fixed gazebo lights tfs (`#377 `_) diff --git a/panther_description/CMakeLists.txt b/panther_description/CMakeLists.txt index 741a0a391..87ff28f03 100644 --- a/panther_description/CMakeLists.txt +++ b/panther_description/CMakeLists.txt @@ -7,5 +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() diff --git a/panther_description/README.md b/panther_description/README.md index 81a4197bc..6c15c4c40 100644 --- a/panther_description/README.md +++ b/panther_description/README.md @@ -1,3 +1,14 @@ # panther_description The package contains URDF files responsible for creating a representation of the robot by specifying the relationships and types of connections (joints) between individual links. It also contains information about the robot's mesh. + +## Launch Files + +- `load_urdf.launch.py` - loads the robot's URDF and creates simple bindings to display moving joints. + +## Configuration Files + +- [`components.yaml`](./config/components.yaml): Allows you to quickly add visualization of sensors, TF connections and simulate their behavior in the simulator. +- [`WH01.yaml`](./config/WH01.yaml): Description of physical and visual parameters for the wheel WH01. +- [`WH02.yaml`](./config/WH02.yaml): Description of physical and visual parameters for the wheel WH02. +- [`WH04.yaml`](./config/WH04.yaml): Description of physical and visual parameters for the wheel WH04. diff --git a/panther_description/env-hooks/panther_description.sh.in b/panther_description/hooks/panther_description.sh.in similarity index 100% rename from panther_description/env-hooks/panther_description.sh.in rename to panther_description/hooks/panther_description.sh.in diff --git a/panther_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py index d8b89e84c..13a0b83d9 100644 --- a/panther_description/launch/load_urdf.launch.py +++ b/panther_description/launch/load_urdf.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): "add_wheel_joints", default_value="True", description="Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) battery_config_path = LaunchConfiguration("battery_config_path") @@ -78,7 +78,7 @@ def generate_launch_description(): ), description=( "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/_controller.yaml'. You can also specify" + " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify" " the path to your custom controller configuration file here. " ), ) @@ -95,7 +95,7 @@ def generate_launch_description(): "use_sim", default_value="False", description="Whether simulation is used.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) declare_wheel_config_path_arg = DeclareLaunchArgument( @@ -109,7 +109,7 @@ def generate_launch_description(): ), description=( "Path to wheel configuration file. By default, it is located in " - "'panther_description/config/.yaml'. You can also specify the path " + "'panther_description/config/{wheel_type}.yaml'. You can also specify the path " "to your custom wheel configuration file here. " ), ) diff --git a/panther_description/package.xml b/panther_description/package.xml index e99917c5c..8ebdd01da 100644 --- a/panther_description/package.xml +++ b/panther_description/package.xml @@ -2,7 +2,7 @@ panther_description - 2.1.0 + 2.1.1 The panther_description package Husarion Apache License 2.0 diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro index b8b2afdbb..29e3558f0 100644 --- a/panther_description/urdf/gazebo.urdf.xacro +++ b/panther_description/urdf/gazebo.urdf.xacro @@ -6,12 +6,7 @@ - - - - - - + @@ -42,6 +37,9 @@ ${config_file} ${namespace} + gz_ros2_control/e_stop:=hardware/e_stop + gz_ros2_control/e_stop_reset:=hardware/e_stop_reset + gz_ros2_control/e_stop_trigger:=hardware/e_stop_trigger imu_broadcaster/imu:=imu/data drive_controller/cmd_vel_unstamped:=cmd_vel drive_controller/odom:=odometry/wheels @@ -65,12 +63,7 @@ - - - - - - + @@ -113,9 +106,18 @@ - - + + + + ${name} + ${topic} + ${namespace} + 10 + 0.5 + 0.015 + + true @@ -123,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 60b90ff09..8d2a83207 100644 --- a/panther_description/urdf/panther_macro.urdf.xacro +++ b/panther_description/urdf/panther_macro.urdf.xacro @@ -17,12 +17,7 @@ - - - - - - + @@ -62,7 +57,8 @@ - ign_ros2_control/IgnitionSystem + panther_gazebo/GzPantherSystem + true @@ -189,8 +185,8 @@ - - + + diff --git a/panther_diagnostics/CHANGELOG.rst b/panther_diagnostics/CHANGELOG.rst index d5c07e386..4553018b8 100644 --- a/panther_diagnostics/CHANGELOG.rst +++ b/panther_diagnostics/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package panther_diagnostics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.1 (2024-09-05) +------------------ +* Merge branch 'ros2-devel' into ros2-ns-refactor +* Merge pull request `#349 `_ from husarion/ros2-testing-poc +* Review changes +* Add integration tests condition +* Formatting +* Implement filesystem unit tests +* Round temperature precision +* System monitor improvements +* Extend filesystem responsibility +* Merge branch 'ros2-devel' of https://github.com/husarion/panther_ros into ros2-add-nmea-gps +* Merge remote-tracking branch 'origin/ros2-devel' into ros2-testing-poc +* Merge pull request `#386 `_ from husarion/ros2-unify-filenames +* Minor modifications +* Ros2 estop sim gui (`#384 `_) +* Correct include guards in manager package +* Merge branch 'ros2-devel' into ros2-ns-refactor +* Rename files in panther_diagnostics package +* unify CMakeLists.txt files (`#381 `_) +* unify CMakeLists.txt files +* New format of documentation (`#369 `_) +* Implement testing POC +* Contributors: Dawid, Dawid Kmak, KmakD, Paweł Irzyk, pawelirh, rafal-gorecki + 2.1.0 (2024-08-02) ------------------ * Ros2 system status tf namespace (`#372 `_) diff --git a/panther_diagnostics/CMakeLists.txt b/panther_diagnostics/CMakeLists.txt index 6c73bb2f4..8794efbed 100644 --- a/panther_diagnostics/CMakeLists.txt +++ b/panther_diagnostics/CMakeLists.txt @@ -15,55 +15,67 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_INCLUDE_DEPENDS +set(PACKAGE_DEPENDENCIES ament_cmake - rclcpp - panther_msgs - panther_utils - diagnostic_updater diagnostic_msgs - std_msgs + diagnostic_updater generate_parameter_library - PkgConfig) + panther_msgs + panther_utils + PkgConfig + rclcpp + std_msgs) -foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) endforeach() include_directories(include) set(CPPUPROFILE_PREFIX ${CMAKE_BINARY_DIR}/ep_cppuprofile/src/ep_cppuprofile) set(ENV{PKG_CONFIG_PATH} "${CPPUPROFILE_PREFIX}/lib:$ENV{PKG_CONFIG_PATH}") + pkg_check_modules(CPPUPROFILE REQUIRED IMPORTED_TARGET cppuprofile) -generate_parameter_library(system_status_parameters - config/system_status_parameters.yaml) +generate_parameter_library(system_monitor_parameters config/system_monitor.yaml) -add_executable(system_status_node src/main.cpp src/system_status_node.cpp) -target_include_directories(system_status_node +add_executable(system_monitor_node src/main.cpp src/system_monitor_node.cpp) +target_include_directories(system_monitor_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include) - -ament_target_dependencies(system_status_node ${PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(system_status_node system_status_parameters +ament_target_dependencies(system_monitor_node ${PACKAGE_DEPENDENCIES}) +target_link_libraries(system_monitor_node system_monitor_parameters PkgConfig::CPPUPROFILE) -install(DIRECTORY include/ DESTINATION include/) -install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) +install(TARGETS system_monitor_node DESTINATION lib/${PROJECT_NAME}) -install(TARGETS system_status_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - - ament_add_gtest(${PROJECT_NAME}_test_system_status - test/test_system_status_node.cpp src/system_status_node.cpp) - target_include_directories(${PROJECT_NAME}_test_system_status + find_package(ament_cmake_gmock REQUIRED) + find_package(ros_testing REQUIRED) + + # Unit tests + ament_add_gmock( + ${PROJECT_NAME}_test_system_monitor test/unit/test_system_monitor_node.cpp + src/system_monitor_node.cpp) + target_include_directories(${PROJECT_NAME}_test_system_monitor PUBLIC ${CMAKE_INSTALL_PREFIX}/include) + ament_target_dependencies(${PROJECT_NAME}_test_system_monitor + ament_cmake_gtest ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_system_monitor + system_monitor_parameters PkgConfig::CPPUPROFILE) - ament_target_dependencies(${PROJECT_NAME}_test_system_status - ament_cmake_gtest ${PACKAGE_INCLUDE_DEPENDS}) - target_link_libraries(${PROJECT_NAME}_test_system_status - system_status_parameters PkgConfig::CPPUPROFILE) + ament_add_gmock(${PROJECT_NAME}_test_filesystem test/unit/test_filesystem.cpp) + target_include_directories(${PROJECT_NAME}_test_filesystem + PUBLIC ${CMAKE_INSTALL_PREFIX}/include) + ament_target_dependencies(${PROJECT_NAME}_test_filesystem ament_cmake_gtest + ${PACKAGE_DEPENDENCIES}) + + # Integration tests + option(TEST_INTEGRATION "Run integration tests" ON) + if(TEST_INTEGRATION) + add_ros_test(test/integration/system_monitor_node.test.py) + endif() endif() diff --git a/panther_diagnostics/README.md b/panther_diagnostics/README.md index 0c85ba3ec..d3e213bbd 100644 --- a/panther_diagnostics/README.md +++ b/panther_diagnostics/README.md @@ -1,50 +1,30 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_diagnostics -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - Package containing nodes monitoring and publishing the Built-in Computer status of Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) +## Launch Files -## ROS Nodes +- `system_monitor.launch.py`: Launch a node that analyzes the state of the most important components in the robot -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +## Configuration Files -### system_monitor +- [`system_monitor.yaml`](./config/system_monitor.yaml): Defines parameters for `system_monitor_node`. -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) +## ROS Nodes -Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and CPU temperature. +### system_monitor_node -[//]: # (ROS_API_NODE_DESCRIPTION_END) +Publishes the built-in computer system status , monitoring parameters as such as CPU usage, RAM usage, disk usage, and CPU temperature. #### Publishes -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: system status diagnostic messages. -- `system_status` [*panther_msgs/SystemStatus*]: system status statistics. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System monitor diagnostic messages. +- `system_status` [*panther_msgs/SystemStatus*]: Built-in computer system status, includes the most important computation-related parameters. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - -- `~frame_id` [*string*, default: **build_in_computer**]: Frame where computer is located. -- `~publish_rate` [*double*, default: **0.25**]: System status publish rate in seconds. -- `~disk_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for disk usage warning in percentage. -- `~memory_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for memory usage warning in percentage. - `~cpu_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for CPU usage warning in percentage. - `~cpu_temperature_warn_threshold` [*float*, default: **80.0**]: Threshold for CPU temperature warning in degrees Celsius. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) +- `~ram_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for memory usage warning in percentage. +- `~disk_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for disk usage warning in percentage. +- `~publish_frequency` [*double*, default: **5.0**]: System status publishing frequency [Hz]. diff --git a/panther_diagnostics/cmake/SuperBuild.cmake b/panther_diagnostics/cmake/SuperBuild.cmake index 1b2c25333..4370b4cdd 100644 --- a/panther_diagnostics/cmake/SuperBuild.cmake +++ b/panther_diagnostics/cmake/SuperBuild.cmake @@ -20,11 +20,11 @@ list(APPEND DEPENDENCIES ep_cppuprofile) ExternalProject_Add( ep_cppuprofile + INSTALL_DIR ${CMAKE_INSTALL_PREFIX} GIT_REPOSITORY https://github.com/Orange-OpenSource/cppuprofile.git GIT_TAG 1.1.1 PREFIX ${CMAKE_CURRENT_BINARY_DIR}/ep_cppuprofile BUILD_COMMAND $(MAKE) -C - INSTALL_DIR ${CMAKE_INSTALL_PREFIX} INSTALL_COMMAND make install INSTALL_PREFIX= CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} BUILD_IN_SOURCE 1) diff --git a/panther_diagnostics/config/system_monitor.yaml b/panther_diagnostics/config/system_monitor.yaml new file mode 100644 index 000000000..7253eea5b --- /dev/null +++ b/panther_diagnostics/config/system_monitor.yaml @@ -0,0 +1,26 @@ +system_monitor: + cpu_usage_warn_threshold: + type: double + default_value: 95.0 + description: Threshold for CPU usage warning in percentage. + validation: { bounds<>: [0.0, 100.0] } + cpu_temperature_warn_threshold: + type: double + default_value: 80.0 + description: Threshold for CPU temperature warning in degrees Celsius. + validation: { bounds<>: [0.0, 1000.0] } + ram_usage_warn_threshold: + type: double + default_value: 95.0 + description: Threshold for memory usage warning in percentage. + validation: { bounds<>: [0.0, 100.0] } + disk_usage_warn_threshold: + type: double + default_value: 95.0 + description: Threshold for disk usage warning in percentage. + validation: { bounds<>: [0.0, 100.0] } + publish_frequency: + type: double + default_value: 5.0 + description: System status publishing frequency [Hz]. + validation: { bounds<>: [0.1, 10.0] } diff --git a/panther_diagnostics/config/system_status_parameters.yaml b/panther_diagnostics/config/system_status_parameters.yaml deleted file mode 100644 index 0fddc5f4e..000000000 --- a/panther_diagnostics/config/system_status_parameters.yaml +++ /dev/null @@ -1,44 +0,0 @@ -system_status: - frame_id: - type: string - default_value: built_in_computer - description: Frame where computer is located. - hardware_id: - type: string - default_value: Built-in Computer - description: Name of hardware used in diagnostics. - publish_rate: - type: double - default_value: 0.25 - description: System status publish rate in seconds. - validation: { - bounds<>: [0.0, 1000.0] - } - disk_usage_warn_threshold: - type: double - default_value: 95.0 - description: Threshold for disk usage warning in percentage. - validation: { - bounds<>: [0.0, 100.0] - } - memory_usage_warn_threshold: - type: double - default_value: 95.0 - description: Threshold for memory usage warning in percentage. - validation: { - bounds<>: [0.0, 100.0] - } - cpu_usage_warn_threshold: - type: double - default_value: 95.0 - description: Threshold for CPU usage warning in percentage. - validation: { - bounds<>: [0.0, 100.0] - } - cpu_temperature_warn_threshold: - type: double - default_value: 80.0 - description: Threshold for CPU temperature warning in degrees Celsius. - validation: { - bounds<>: [0.0, 1000.0] - } diff --git a/panther_diagnostics/include/panther_diagnostics/filesystem.hpp b/panther_diagnostics/include/panther_diagnostics/filesystem.hpp new file mode 100644 index 000000000..fa2291d62 --- /dev/null +++ b/panther_diagnostics/include/panther_diagnostics/filesystem.hpp @@ -0,0 +1,115 @@ +// 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_DIAGNOSTICS_FILESYSTEM_HPP_ +#define PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_ + +#include +#include + +namespace panther_diagnostics +{ + +/** + * @brief Abstract interface for the filesystem methods. + */ +class FilesystemInterface +{ +public: + /** + * @brief Virtual destructor for the FilesystemInterface class. + */ + virtual ~FilesystemInterface() = default; + + virtual uintmax_t GetSpaceCapacity(const std::string & filesystem_path) const = 0; + + virtual uintmax_t GetSpaceAvailable(const std::string & filesystem_path) const = 0; + + virtual std::string ReadFile(const std::string & file_path) const = 0; + + /** + * @brief Alias for a shared pointer to a FilesystemInterface object. + */ + using SharedPtr = std::shared_ptr; +}; + +/** + * @brief A class that provides functionality for interacting with the filesystem. + * + * This class inherits from the `FilesystemInterface` and implements its methods. + * It provides a simplified, facade-type way to interact with the `std::filesystem` library. + */ +class Filesystem : public FilesystemInterface +{ +public: + /** + * @brief Returns the space capacity in bytes of the filesystem at the specified path. + * + * @param filesystem_path The path to the filesystem. + * @return The space capacity in bytes. + */ + inline uintmax_t GetSpaceCapacity(const std::string & filesystem_path) const override + { + const auto path = std::filesystem::path(filesystem_path); + const auto space_info = std::filesystem::space(path); + + return space_info.capacity; + } + + /** + * @brief Returns the available space in bytes of the filesystem at the specified path. + * + * @param filesystem_path The path to the filesystem. + * @return The available space in bytes. + */ + inline uintmax_t GetSpaceAvailable(const std::string & filesystem_path) const override + { + const auto path = std::filesystem::path(filesystem_path); + const auto space_info = std::filesystem::space(path); + + return space_info.available; + } + + /** + * @brief Reads the contents of the file specified by the given file path. + * + * @param file_path The path to the file to be read. + * @return The contents of the file as a string. + * @throws `std::invalid_argument` If the file doesn't exist. + * @throws `std::runtime_error` If the file fails to open. + */ + std::string ReadFile(const std::string & file_path) const override + { + const auto path = std::filesystem::path(file_path); + + if (!std::filesystem::exists(path)) { + throw std::invalid_argument("File doesn't exist, given path " + path.string()); + } + + std::ifstream file(path); + if (!file.is_open()) { + throw std::runtime_error("Failed to open, given path " + path.string()); + } + + std::stringstream buffer; + buffer << file.rdbuf(); + + file.close(); + return buffer.str(); + } +}; + +} // namespace panther_diagnostics + +#endif // PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_ diff --git a/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp b/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp similarity index 51% rename from panther_diagnostics/include/panther_diagnostics/system_status_node.hpp rename to panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp index 235d5d417..c840a246b 100644 --- a/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp +++ b/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp @@ -12,59 +12,74 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_DIAGNOSTICS_SYSTEM_STATUS_NODE_HPP_ -#define PANTHER_DIAGNOSTICS_SYSTEM_STATUS_NODE_HPP_ +#ifndef PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ +#define PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ #include -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include #include "panther_msgs/msg/system_status.hpp" -#include "system_status_parameters.hpp" +#include "system_monitor_parameters.hpp" + +#include "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/types.hpp" using namespace std::chrono_literals; namespace panther_diagnostics { -class SystemStatusNode : public rclcpp::Node +class SystemMonitorNode : public rclcpp::Node { public: - SystemStatusNode(const std::string & node_name); - - struct SystemStatus - { - std::vector core_usages; - float cpu_mean_usage; - float cpu_temperature; - float memory_usage; - float disk_usage; - }; + SystemMonitorNode( + const std::string & node_name, FilesystemInterface::SharedPtr filesystem, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); protected: + /** + * @brief Retrieves the system parameters and generate system status object describing the current + * system state. + * + * @return The system status. + */ SystemStatus GetSystemStatus() const; + std::vector GetCoresUsages() const; float GetCPUMeanUsage(const std::vector & usages) const; - float GetCPUTemperature(const std::string & filename) const; - float GetMemoryUsage() const; + float GetCPUTemperature() const; + float GetRAMUsage() const; float GetDiskUsage() const; + /** + * @brief Converts a SystemStatus object to a SystemStatus message. + * + * This function takes a SystemStatus object and converts it into a SystemStatus message. + * The resulting message can be used to publish the system status over a ROS topic. + * + * @param status The SystemStatus object to be converted. + * @return The converted SystemStatus message. + */ panther_msgs::msg::SystemStatus SystemStatusToMessage(const SystemStatus & status); private: void TimerCallback(); void DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapper & status); + FilesystemInterface::SharedPtr filesystem_; + diagnostic_updater::Updater diagnostic_updater_; + rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr system_status_publisher_; - diagnostic_updater::Updater diagnostic_updater_; - system_status::Params params_; - std::shared_ptr param_listener_; + system_monitor::Params params_; + std::shared_ptr param_listener_; static constexpr char kTemperatureInfoFilename[] = "/sys/class/thermal/thermal_zone0/temp"; + static constexpr char kRootDirectory[] = "/"; }; } // namespace panther_diagnostics -#endif // PANTHER_DIAGNOSTICS_SYSTEM_STATUS_NODE_HPP_ +#endif // PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ diff --git a/panther_diagnostics/include/panther_diagnostics/types.hpp b/panther_diagnostics/include/panther_diagnostics/types.hpp new file mode 100644 index 000000000..1ed2c88c7 --- /dev/null +++ b/panther_diagnostics/include/panther_diagnostics/types.hpp @@ -0,0 +1,40 @@ +// 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_DIAGNOSTICS_TYPES_HPP_ +#define PANTHER_DIAGNOSTICS_TYPES_HPP_ + +#include + +namespace panther_diagnostics +{ + +/** + * @brief Represents the system status information. + * + * This struct contains various metrics related to the system status, such as CPU usage, CPU + * temperature, memory usage, and disk usage. + */ +struct SystemStatus +{ + std::vector core_usages; + float cpu_mean_usage; + float cpu_temperature; + float ram_usage; + float disk_usage; +}; + +} // namespace panther_diagnostics + +#endif // PANTHER_DIAGNOSTICS_TYPES_HPP_ diff --git a/panther_diagnostics/launch/system_status.launch.py b/panther_diagnostics/launch/system_monitor.launch.py similarity index 93% rename from panther_diagnostics/launch/system_status.launch.py rename to panther_diagnostics/launch/system_monitor.launch.py index 103acf598..e2d2ab706 100644 --- a/panther_diagnostics/launch/system_status.launch.py +++ b/panther_diagnostics/launch/system_monitor.launch.py @@ -28,9 +28,9 @@ def generate_launch_description(): description="Add namespace to all launched nodes", ) - system_status_node = Node( + system_monitor_node = Node( package="panther_diagnostics", - executable="system_status_node", + executable="system_monitor_node", name="system_monitor", namespace=namespace, remappings=[("/diagnostics", "diagnostics")], @@ -39,7 +39,7 @@ def generate_launch_description(): actions = [ declare_namespace_arg, - system_status_node, + system_monitor_node, ] return LaunchDescription(actions) diff --git a/panther_diagnostics/package.xml b/panther_diagnostics/package.xml index e6f1ba827..9f827b6d8 100644 --- a/panther_diagnostics/package.xml +++ b/panther_diagnostics/package.xml @@ -2,7 +2,7 @@ panther_diagnostics - 2.1.0 + 2.1.1 Package for diagnosting usage of OS on the Panther Robot Husarion Apache License 2.0 @@ -12,6 +12,7 @@ https://github.com/husarion/panther_ros/issues Jakub Delicat + Pawel Irzyk ament_cmake pkg-config @@ -25,6 +26,8 @@ std_msgs ament_cmake_gtest + google-mock + ros_testing ament_cmake diff --git a/panther_diagnostics/src/main.cpp b/panther_diagnostics/src/main.cpp index 69059d00c..8d09517f6 100644 --- a/panther_diagnostics/src/main.cpp +++ b/panther_diagnostics/src/main.cpp @@ -16,25 +16,27 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include -#include "panther_diagnostics/system_status_node.hpp" +#include "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/system_monitor_node.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto system_status_node = - std::make_shared("system_monitor"); + auto filesystem = std::make_shared(); + auto system_monitor_node = std::make_shared( + "system_monitor", filesystem); try { - rclcpp::spin(system_status_node); + rclcpp::spin(system_monitor_node); } catch (const std::runtime_error & e) { - std::cerr << "[" << system_status_node->get_name() << "] Caught exception: " << e.what() + std::cerr << "[" << system_monitor_node->get_name() << "] Caught exception: " << e.what() << std::endl; } - std::cout << "[" << system_status_node->get_name() << "] Shutting down" << std::endl; + std::cout << "[" << system_monitor_node->get_name() << "] Shutting down" << std::endl; rclcpp::shutdown(); return 0; } diff --git a/panther_diagnostics/src/system_monitor_node.cpp b/panther_diagnostics/src/system_monitor_node.cpp new file mode 100644 index 000000000..bc921a75d --- /dev/null +++ b/panther_diagnostics/src/system_monitor_node.cpp @@ -0,0 +1,199 @@ +// 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. + +#include "panther_diagnostics/system_monitor_node.hpp" + +#include +#include + +#include + +#include +#include + +#include "panther_msgs/msg/system_status.hpp" + +#include "panther_utils/common_utilities.hpp" +#include "panther_utils/ros_utils.hpp" + +#include "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/types.hpp" + +namespace panther_diagnostics +{ + +SystemMonitorNode::SystemMonitorNode( + const std::string & node_name, FilesystemInterface::SharedPtr filesystem, + const rclcpp::NodeOptions & options) +: rclcpp::Node(node_name, options), filesystem_(filesystem), diagnostic_updater_(this) +{ + RCLCPP_INFO(this->get_logger(), "Initializing."); + + param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + params_ = param_listener_->get_params(); + + system_status_publisher_ = this->create_publisher( + "system_status", 10); + + const auto timer_interval_ms = static_cast(1000.0 / params_.publish_frequency); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(timer_interval_ms), + std::bind(&SystemMonitorNode::TimerCallback, this)); + + diagnostic_updater_.setHardwareID("Built In Computer"); + diagnostic_updater_.add("OS status", this, &SystemMonitorNode::DiagnoseSystem); + + RCLCPP_INFO(this->get_logger(), "Initialized successfully."); +} + +void SystemMonitorNode::TimerCallback() +{ + const auto status = GetSystemStatus(); + auto message = SystemStatusToMessage(status); + + system_status_publisher_->publish(message); +} + +SystemStatus SystemMonitorNode::GetSystemStatus() const +{ + SystemStatus status; + + status.core_usages = GetCoresUsages(); + status.cpu_mean_usage = GetCPUMeanUsage(status.core_usages); + status.cpu_temperature = GetCPUTemperature(); + status.ram_usage = GetRAMUsage(); + status.disk_usage = GetDiskUsage(); + + return status; +} + +std::vector SystemMonitorNode::GetCoresUsages() const +{ + std::vector loads = uprofile::getInstantCpuUsage(); + + return loads; +} + +float SystemMonitorNode::GetCPUMeanUsage(const std::vector & usages) const +{ + if (usages.empty()) { + return std::numeric_limits::quiet_NaN(); + } + + std::for_each(usages.begin(), usages.end(), [](const float & usage) { + if (usage < 0.0 || usage > 100.0) { + throw std::invalid_argument{ + "At least one CPU core exceeds the valid usage range [0.0, 100.0]."}; + }; + }); + + auto sum = std::accumulate(usages.begin(), usages.end(), 0.0); + auto mean_usage = panther_utils::common_utilities::SetPrecision(sum / usages.size(), 2); + + return mean_usage; +} + +float SystemMonitorNode::GetCPUTemperature() const +{ + float temperature = std::numeric_limits::quiet_NaN(); + + try { + const auto temperature_str = filesystem_->ReadFile(kTemperatureInfoFilename); + temperature = panther_utils::common_utilities::SetPrecision( + std::stof(temperature_str) / 1000.0, 2); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "An exception occurred while reading CPU temperature: " << e.what()); + } + + return temperature; +} + +float SystemMonitorNode::GetRAMUsage() const +{ + int total = 0, free = 0, available = 0; + uprofile::getSystemMemory(total, available, free); + + const auto ram_usage = panther_utils::common_utilities::CountPercentage(total - available, total); + return ram_usage; +} + +float SystemMonitorNode::GetDiskUsage() const +{ + float disk_usage = std::numeric_limits::quiet_NaN(); + + try { + const auto capacity = filesystem_->GetSpaceCapacity(kRootDirectory); + const auto available = filesystem_->GetSpaceAvailable(kRootDirectory); + disk_usage = panther_utils::common_utilities::CountPercentage(capacity - available, capacity); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "An exception occurred while reading disk usage: " << e.what()); + } + + return disk_usage; +} + +panther_msgs::msg::SystemStatus SystemMonitorNode::SystemStatusToMessage( + const SystemStatus & status) +{ + panther_msgs::msg::SystemStatus message; + + message.header.stamp = this->get_clock()->now(); + message.cpu_percent = status.core_usages; + message.avg_load_percent = status.cpu_mean_usage; + message.cpu_temp = status.cpu_temperature; + message.ram_usage_percent = status.ram_usage; + message.disc_usage_percent = status.disk_usage; + + return message; +} + +void SystemMonitorNode::DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + auto error_level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string message = "System parameters are within acceptable limits."; + + params_ = param_listener_->get_params(); + auto system_status = GetSystemStatus(); + + status.add("CPU usage (%)", system_status.cpu_mean_usage); + status.add("CPU temperature (°C)", system_status.cpu_temperature); + status.add("RAM memory usage (%)", system_status.ram_usage); + status.add("Disk memory usage (%)", system_status.disk_usage); + + std::unordered_map limits = { + {params_.cpu_usage_warn_threshold, status.values[0]}, + {params_.cpu_temperature_warn_threshold, status.values[1]}, + {params_.ram_usage_warn_threshold, status.values[2]}, + {params_.disk_usage_warn_threshold, status.values[3]}, + }; + + for (const auto & [limit, key_value] : limits) { + if (std::isnan(std::stod(key_value.value))) { + message = "Detected system parameter with unknown value."; + error_level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; + break; + } else if (std::stod(key_value.value) > limit) { + message = "At least one system parameter is above the warning threshold."; + error_level = diagnostic_updater::DiagnosticStatusWrapper::WARN; + } + } + + status.summary(error_level, message); +} + +} // namespace panther_diagnostics diff --git a/panther_diagnostics/src/system_status_node.cpp b/panther_diagnostics/src/system_status_node.cpp deleted file mode 100644 index 51d1663f8..000000000 --- a/panther_diagnostics/src/system_status_node.cpp +++ /dev/null @@ -1,169 +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. - -#include "panther_diagnostics/system_status_node.hpp" - -#include -#include -#include -#include - -#include "cppuprofile/uprofile.h" - -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "panther_msgs/msg/system_status.hpp" -#include "panther_utils/common_utilities.hpp" -#include "panther_utils/ros_utils.hpp" - -namespace panther_diagnostics -{ - -SystemStatusNode::SystemStatusNode(const std::string & node_name) -: rclcpp::Node(node_name), diagnostic_updater_(this) -{ - RCLCPP_INFO(this->get_logger(), "Initializing."); - - param_listener_ = - std::make_shared(this->get_node_parameters_interface()); - params_ = param_listener_->get_params(); - - system_status_publisher_ = this->create_publisher( - "system_status", 10); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(params_.publish_rate * 1000)), - std::bind(&SystemStatusNode::TimerCallback, this)); - - diagnostic_updater_.setHardwareID(params_.hardware_id); - diagnostic_updater_.add("OS status", this, &SystemStatusNode::DiagnoseSystem); - - RCLCPP_INFO(this->get_logger(), "Initialized successfully."); -} - -void SystemStatusNode::TimerCallback() -{ - const auto status = GetSystemStatus(); - auto message = SystemStatusToMessage(status); - - system_status_publisher_->publish(message); -} - -SystemStatusNode::SystemStatus SystemStatusNode::GetSystemStatus() const -{ - SystemStatusNode::SystemStatus status; - - status.core_usages = GetCoresUsages(); - status.cpu_mean_usage = GetCPUMeanUsage(status.core_usages); - status.cpu_temperature = GetCPUTemperature(kTemperatureInfoFilename); - status.memory_usage = GetMemoryUsage(); - status.disk_usage = GetDiskUsage(); - - return status; -} - -std::vector SystemStatusNode::GetCoresUsages() const -{ - std::vector loads = uprofile::getInstantCpuUsage(); - return loads; -} - -float SystemStatusNode::GetCPUMeanUsage(const std::vector & usages) const -{ - auto sum = std::accumulate(usages.begin(), usages.end(), 0.0); - return sum / usages.size(); -} - -float SystemStatusNode::GetCPUTemperature(const std::string & filename) const -{ - try { - auto file = panther_utils::common_utilities::OpenFile(filename, std::ios_base::in); - float temperature; - file >> temperature; - file.close(); - return temperature / 1000.0; - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "An exception occurred while reading CPU temperature: " << e.what()); - } - return std::numeric_limits::quiet_NaN(); -} - -float SystemStatusNode::GetMemoryUsage() const -{ - int total = 0, free = 0, available = 0; - uprofile::getSystemMemory(total, free, available); - return static_cast(total - available) / total * 100.0; -} - -float SystemStatusNode::GetDiskUsage() const -{ - const std::filesystem::directory_entry entry("/"); - const std::filesystem::space_info si = std::filesystem::space(entry.path()); - - return static_cast(si.capacity - si.available) / si.capacity * 100.0; -} - -panther_msgs::msg::SystemStatus SystemStatusNode::SystemStatusToMessage( - const SystemStatusNode::SystemStatus & status) -{ - panther_msgs::msg::SystemStatus message; - - message.header.stamp = this->get_clock()->now(); - message.header.frame_id = panther_utils::ros::AddNamespaceToFrameID( - params_.frame_id, std::string(this->get_namespace())); - message.cpu_percent = status.core_usages; - message.avg_load_percent = status.cpu_mean_usage; - message.cpu_temp = status.cpu_temperature; - message.ram_usage_percent = status.memory_usage; - message.disc_usage_percent = status.disk_usage; - - return message; -} - -void SystemStatusNode::DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapper & status) -{ - auto error_level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string message = "System parameters are within acceptable limits."; - - params_ = param_listener_->get_params(); - auto system_status = GetSystemStatus(); - - status.add("CPU usage", system_status.cpu_mean_usage); - status.add("CPU temperature", system_status.cpu_temperature); - status.add("Disk memory usage", system_status.disk_usage); - status.add("RAM memory usage", system_status.memory_usage); - - std::unordered_map limits = { - {params_.cpu_usage_warn_threshold, status.values[0]}, - {params_.cpu_temperature_warn_threshold, status.values[1]}, - {params_.disk_usage_warn_threshold, status.values[2]}, - {params_.memory_usage_warn_threshold, status.values[3]}, - }; - - for (const auto & [limit, key_value] : limits) { - if (std::isnan(std::stod(key_value.value))) { - message = "Detected system parameter with unknown value."; - error_level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; - break; - } else if (std::stod(key_value.value) > limit) { - message = "At least one system parameter is above the warning threshold."; - error_level = diagnostic_updater::DiagnosticStatusWrapper::WARN; - } - } - - status.summary(error_level, message); -} - -} // namespace panther_diagnostics diff --git a/panther_diagnostics/test/integration/system_monitor_node.test.py b/panther_diagnostics/test/integration/system_monitor_node.test.py new file mode 100644 index 000000000..6307e9724 --- /dev/null +++ b/panther_diagnostics/test/integration/system_monitor_node.test.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# 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. + +import unittest + +import launch +import launch_testing +import pytest +from diagnostic_msgs.msg import DiagnosticArray +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing_ros import WaitForTopics +from panther_msgs.msg import SystemStatus + + +@pytest.mark.launch_test +def generate_test_description(): + + system_monitor_node = Node( + package="panther_diagnostics", + executable="system_monitor_node", + ) + + # Start test after 1s + delay_timer = launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ) + + actions = [system_monitor_node, delay_timer] + + context = {} + + return ( + LaunchDescription(actions), + context, + ) + + +class TestNodeInitialization(unittest.TestCase): + def test_initialization_log(self, proc_output): + proc_output.assertWaitFor("Initialized successfully.") + + +class TestNode(unittest.TestCase): + def test_msg(self): + topic_list = [("system_status", SystemStatus), ("diagnostics", DiagnosticArray)] + + with WaitForTopics(topic_list, timeout=5.0) as wait_for_topics: + received_topics_str = ", ".join(wait_for_topics.topics_received()) + print("Received messages from the following topics: [" + received_topics_str + "]") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/panther_diagnostics/test/test_system_status_node.cpp b/panther_diagnostics/test/test_system_status_node.cpp deleted file mode 100644 index 9f606cc55..000000000 --- a/panther_diagnostics/test/test_system_status_node.cpp +++ /dev/null @@ -1,142 +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. - -#include -#include -#include -#include - -#include "gtest/gtest.h" - -#include "panther_diagnostics/system_status_node.hpp" - -class SystemStatusNodeWrapper : public panther_diagnostics::SystemStatusNode -{ -public: - SystemStatusNodeWrapper() : panther_diagnostics::SystemStatusNode("test_system_statics") {} - - std::vector GetCoresUsages() const - { - return panther_diagnostics::SystemStatusNode::GetCoresUsages(); - } - - float GetCPUMeanUsage(const std::vector & usages) const - { - return panther_diagnostics::SystemStatusNode::GetCPUMeanUsage(usages); - } - - float GetCPUTemperature(const std::string & filename) const - { - return panther_diagnostics::SystemStatusNode::GetCPUTemperature(filename); - } - - float GetMemoryUsage() const { return panther_diagnostics::SystemStatusNode::GetMemoryUsage(); } - - float GetDiskUsage() const { return panther_diagnostics::SystemStatusNode::GetDiskUsage(); } - - panther_msgs::msg::SystemStatus SystemStatusToMessage( - const panther_diagnostics::SystemStatusNode::SystemStatus & status) - { - return panther_diagnostics::SystemStatusNode::SystemStatusToMessage(status); - } -}; - -class TestSystemStatusNode : public testing::Test -{ -public: - TestSystemStatusNode(); - -protected: - std::unique_ptr system_status_; -}; - -TestSystemStatusNode::TestSystemStatusNode() -{ - system_status_ = std::make_unique(); -} - -TEST_F(TestSystemStatusNode, CheckCoresUsages) -{ - const auto usages = system_status_->GetCoresUsages(); - - for (const auto & usage : usages) { - EXPECT_TRUE((usage >= 0.0) && (usage <= 100.0)); - } -} - -TEST_F(TestSystemStatusNode, CheckCPUMeanUsage) -{ - std::vector usages = {45.0, 55.0, 45.0, 55.0}; - - const auto mean = system_status_->GetCPUMeanUsage(usages); - EXPECT_FLOAT_EQ(mean, 50.0); -} - -TEST_F(TestSystemStatusNode, CheckTemperatureReadings) -{ - const std::string temperature_file_name = testing::TempDir() + "panther_diagnostics_temperature"; - - // Make sure that there is no random file with random value. - std::filesystem::remove(temperature_file_name); - - std::ofstream temperature_file(temperature_file_name, std::ofstream::out); - temperature_file << 36600 << std::endl; - temperature_file.close(); - - const auto temperature = system_status_->GetCPUTemperature(temperature_file_name); - std::filesystem::remove(temperature_file_name); - - EXPECT_FLOAT_EQ(temperature, 36.6); -} - -TEST_F(TestSystemStatusNode, CheckMemoryReadings) -{ - const auto memory = system_status_->GetMemoryUsage(); - - EXPECT_TRUE((memory >= 0.0) && (memory <= 100.0)); -} - -TEST_F(TestSystemStatusNode, CheckDiskReadings) -{ - const auto disk_usage = system_status_->GetDiskUsage(); - - EXPECT_TRUE((disk_usage >= 0.0) && (disk_usage <= 100.0)); -} - -TEST_F(TestSystemStatusNode, CheckSystemStatusToMessage) -{ - panther_diagnostics::SystemStatusNode::SystemStatus status; - status.core_usages = {50.0, 50.0, 50.0}; - status.cpu_mean_usage = 50.0; - status.cpu_temperature = 36.6; - status.disk_usage = 60.0; - status.memory_usage = 30.0; - - const auto message = system_status_->SystemStatusToMessage(status); - - EXPECT_EQ(message.cpu_percent, status.core_usages); - EXPECT_FLOAT_EQ(message.avg_load_percent, status.cpu_mean_usage); - EXPECT_FLOAT_EQ(message.cpu_temp, status.cpu_temperature); - EXPECT_FLOAT_EQ(message.disc_usage_percent, status.disk_usage); - EXPECT_FLOAT_EQ(message.ram_usage_percent, status.memory_usage); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - rclcpp::init(0, nullptr); - auto result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/panther_diagnostics/test/unit/test_filesystem.cpp b/panther_diagnostics/test/unit/test_filesystem.cpp new file mode 100644 index 000000000..3e9c87553 --- /dev/null +++ b/panther_diagnostics/test/unit/test_filesystem.cpp @@ -0,0 +1,98 @@ +// 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. + +#include +#include +#include +#include +#include + +#include "panther_diagnostics/filesystem.hpp" + +class TestFilesystem : public testing::Test +{ +public: + TestFilesystem(); + + std::string CreateTestFile(const std::string & content); + void RemoveTestFile(const std::string & file_path); + +protected: + std::shared_ptr filesystem_; + + static constexpr char kDummyString[] = "Hello World!"; +}; + +TestFilesystem::TestFilesystem() : filesystem_(std::make_shared()) +{ +} + +std::string TestFilesystem::CreateTestFile(const std::string & content) +{ + const std::string file_path = testing::TempDir() + "test_file.txt"; + + std::ofstream file(file_path); + file << content; + file.close(); + + return file_path; +} + +void TestFilesystem::RemoveTestFile(const std::string & file_path) +{ + std::remove(file_path.c_str()); +} + +TEST_F(TestFilesystem, ReadFileSuccess) +{ + const auto test_file_path = CreateTestFile(kDummyString); + + auto content = filesystem_->ReadFile(test_file_path); + + EXPECT_STREQ(kDummyString, content.c_str()); + + RemoveTestFile(test_file_path); +} + +TEST_F(TestFilesystem, ReadFileNonExistent) +{ + // File does not exist + auto const test_file_path = testing::TempDir() + "test_missing_file.txt"; + + EXPECT_THROW(filesystem_->ReadFile(test_file_path), std::invalid_argument); +} + +TEST_F(TestFilesystem, ReadFileLocked) +{ + const auto test_file_path = CreateTestFile(kDummyString); + + // Change file permissions to make it unreadable + std::filesystem::permissions(test_file_path, std::filesystem::perms::none); + + EXPECT_THROW(filesystem_->ReadFile(test_file_path), std::runtime_error); + + // Restore file permissions to allow deletion + std::filesystem::permissions(test_file_path, std::filesystem::perms::owner_all); + + RemoveTestFile(test_file_path); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + auto result = RUN_ALL_TESTS(); + + return result; +} diff --git a/panther_diagnostics/test/unit/test_system_monitor_node.cpp b/panther_diagnostics/test/unit/test_system_monitor_node.cpp new file mode 100644 index 000000000..8c93ff248 --- /dev/null +++ b/panther_diagnostics/test/unit/test_system_monitor_node.cpp @@ -0,0 +1,188 @@ +// 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. + +#include +#include +#include +#include + +#include +#include + +#include "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/system_monitor_node.hpp" + +class MockFilesystem : public panther_diagnostics::FilesystemInterface +{ +public: + MOCK_METHOD( + uintmax_t, GetSpaceCapacity, (const std::string & filesystem_path), (const, override)); + + MOCK_METHOD( + uintmax_t, GetSpaceAvailable, (const std::string & filesystem_path), (const, override)); + + MOCK_METHOD(std::string, ReadFile, (const std::string & file_path), (const, override)); +}; + +class SystemMonitorNodeWrapper : public panther_diagnostics::SystemMonitorNode +{ +public: + SystemMonitorNodeWrapper(std::shared_ptr filesystem) + : panther_diagnostics::SystemMonitorNode("test_system_statics", filesystem) + { + } + + std::vector GetCoresUsages() const + { + return panther_diagnostics::SystemMonitorNode::GetCoresUsages(); + } + + float GetCPUMeanUsage(const std::vector & usages) const + { + return panther_diagnostics::SystemMonitorNode::GetCPUMeanUsage(usages); + } + + float GetCPUTemperature() const + { + return panther_diagnostics::SystemMonitorNode::GetCPUTemperature(); + } + + float GetRAMUsage() const { return panther_diagnostics::SystemMonitorNode::GetRAMUsage(); } + + float GetDiskUsage() const { return panther_diagnostics::SystemMonitorNode::GetDiskUsage(); } + + panther_msgs::msg::SystemStatus SystemStatusToMessage( + const panther_diagnostics::SystemStatus & status) + + { + return panther_diagnostics::SystemMonitorNode::SystemStatusToMessage(status); + } +}; + +class TestSystemMonitorNode : public testing::Test +{ +public: + TestSystemMonitorNode(); + +protected: + std::shared_ptr filesystem_; + std::unique_ptr system_monitor_; +}; + +TestSystemMonitorNode::TestSystemMonitorNode() +{ + filesystem_ = std::make_unique(); + system_monitor_ = std::make_unique(filesystem_); +} + +TEST_F(TestSystemMonitorNode, GetCPUMeanUsageCorrectInput) +{ + const std::vector test_usages = {100.0, 0.0, 45.0, 55.0}; + const auto expected_mean_usage = 50.0; + + const auto mean_usage = system_monitor_->GetCPUMeanUsage(test_usages); + EXPECT_FLOAT_EQ(expected_mean_usage, mean_usage); +} + +TEST_F(TestSystemMonitorNode, GetCPUMeanUsageEmptyInput) +{ + const std::vector test_usages = {}; + + const auto mean_usage = system_monitor_->GetCPUMeanUsage(test_usages); + + EXPECT_TRUE(std::isnan(mean_usage)); +} + +TEST_F(TestSystemMonitorNode, GetCPUMeanUsageOverloaded) +{ + const std::vector test_usages = {150.0, 50.0, 50.0, 50.0}; + + EXPECT_THROW(system_monitor_->GetCPUMeanUsage(test_usages), std::invalid_argument); +} + +TEST_F(TestSystemMonitorNode, GetCPUMeanUsageUnderloaded) +{ + const std::vector test_usages = {-50.0, 50.0, 50.0, 50.0}; + + EXPECT_THROW(system_monitor_->GetCPUMeanUsage(test_usages), std::invalid_argument); +} + +TEST_F(TestSystemMonitorNode, GetCPUTemperatureValidFile) +{ + ON_CALL(*filesystem_, ReadFile(testing::_)).WillByDefault(testing::Return("25000")); + + const auto temperature = system_monitor_->GetCPUTemperature(); + + EXPECT_EQ(25.0, temperature); +} + +TEST_F(TestSystemMonitorNode, GetCPUTemperatureMissingFile) +{ + ON_CALL(*filesystem_, ReadFile(testing::_)) + .WillByDefault(testing::Throw(std::invalid_argument("File not found"))); + + const auto temperature = system_monitor_->GetCPUTemperature(); + + EXPECT_TRUE(std::isnan(temperature)); +} + +TEST_F(TestSystemMonitorNode, GetDiskUsageValidFilesystem) +{ + ON_CALL(*filesystem_, GetSpaceCapacity(testing::_)).WillByDefault(testing::Return(100000)); + ON_CALL(*filesystem_, GetSpaceAvailable(testing::_)).WillByDefault(testing::Return(50000)); + + const auto disk_usage = system_monitor_->GetDiskUsage(); + + EXPECT_EQ(50.0, disk_usage); +} + +TEST_F(TestSystemMonitorNode, GetDiskUsageInvalidFilesystem) +{ + ON_CALL(*filesystem_, GetSpaceCapacity(testing::_)).WillByDefault(testing::Return(100000)); + ON_CALL(*filesystem_, GetSpaceAvailable(testing::_)) + .WillByDefault(testing::Throw(std::invalid_argument{"Filesystem not found"})); + + const auto disk_usage = system_monitor_->GetDiskUsage(); + + EXPECT_TRUE(std::isnan(disk_usage)); +} + +TEST_F(TestSystemMonitorNode, SystemStatusToMessage) +{ + panther_diagnostics::SystemStatus test_status; + test_status.core_usages = {50.0, 50.0, 50.0}; + test_status.cpu_mean_usage = 50.0; + test_status.cpu_temperature = 36.6; + test_status.ram_usage = 30.0; + test_status.disk_usage = 60.0; + + const auto message = system_monitor_->SystemStatusToMessage(test_status); + + EXPECT_EQ(test_status.core_usages, message.cpu_percent); + EXPECT_FLOAT_EQ(test_status.cpu_mean_usage, message.avg_load_percent); + EXPECT_FLOAT_EQ(test_status.cpu_temperature, message.cpu_temp); + EXPECT_FLOAT_EQ(test_status.ram_usage, message.ram_usage_percent); + EXPECT_FLOAT_EQ(test_status.disk_usage, message.disc_usage_percent); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(0, nullptr); + + auto result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return result; +} diff --git a/panther_gazebo/CHANGELOG.rst b/panther_gazebo/CHANGELOG.rst index be05cdf50..a11cc3b1b 100644 --- a/panther_gazebo/CHANGELOG.rst +++ b/panther_gazebo/CHANGELOG.rst @@ -2,6 +2,40 @@ Changelog for package panther_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.1 (2024-09-05) +------------------ +* LEDStrip plugin to Gazebo (`#391 `_) +* Merge branch 'ros2-devel' into ros2-ns-refactor +* Merge branch 'ros2-devel' of https://github.com/husarion/panther_ros into ros2-add-nmea-gps +* Merge pull request `#383 `_ from husarion/ros-sim-estop +* Dawid suggestions +* Update panther_gazebo/include/panther_gazebo/gz_panther_system.hpp +* Merge remote-tracking branch 'origin/ros2-devel' into ros2-testing-poc +* Merge pull request `#386 `_ from husarion/ros2-unify-filenames +* delete typo +* Dawid suggestions +* Update panther_gazebo/src/gz_panther_system.cpp +* Update panther_gazebo/src/gz_panther_system.cpp +* Update panther_gazebo/include/panther_gazebo/gz_panther_system.hpp +* Fix links in documentations (`#387 `_) +* Rename PantherSystem -> GzPantherSystem +* Dawid suggestions part 1 +* Change to Estop -> EStop +* Inherit from IgnitionSystem +* Move estop to plugins folder +* Typos in Readme + estop publish on service call +* Add david suggestion and change gui layout +* Update panther_gazebo/panther_hardware_plugins.xml +* Ros2 estop sim gui (`#384 `_) +* Rename config and launch file in manager package +* Merge branch 'ros2-devel' into ros2-ns-refactor +* unify CMakeLists.txt files (`#381 `_) +* Add dependencies +* unify CMakeLists.txt files +* Add EStop to Gazebo +* New format of documentation (`#369 `_) +* Contributors: Dawid, Dawid Kmak, KmakD, Paweł Irzyk, pawelirh, rafal-gorecki + 2.1.0 (2024-08-02) ------------------ * Fixed gazebo lights tfs (`#377 `_) diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt index 5b53b717b..cae0aaa39 100644 --- a/panther_gazebo/CMakeLists.txt +++ b/panther_gazebo/CMakeLists.txt @@ -5,28 +5,91 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(ignition-transport11 QUIET REQUIRED) -find_package(ignition-common4 REQUIRED) -find_package(ignition-msgs8 REQUIRED) -find_package(panther_utils REQUIRED) -find_package(yaml-cpp REQUIRED) +set(PACKAGE_DEPENDENCIES + ament_cmake + hardware_interface + ignition-gazebo6 + ignition-msgs8 + ignition-plugin1 + ignition-transport11 + ign_ros2_control + pluginlib + rclcpp + realtime_tools + std_msgs + std_srvs) -include_directories(include) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() -set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) -set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) -set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) +find_package(Qt5 REQUIRED COMPONENTS Core Quick QuickControls2) -add_executable(gz_led_strip_manager src/main.cpp src/gz_led_strip_manager.cpp - src/gz_led_strip.cpp) +include_directories(include ${Qt5Core_INCLUDE_DIRS} ${Qt5Qml_INCLUDE_DIRS} + ${Qt5Quick_INCLUDE_DIRS} ${Qt5QuickControls2_INCLUDE_DIRS}) + +add_library(panther_simulation_plugins SHARED src/gz_panther_system.cpp) +ament_target_dependencies( + panther_simulation_plugins + hardware_interface + ign_ros2_control + rclcpp + rclcpp_lifecycle + std_msgs + std_srvs) +target_link_libraries(panther_simulation_plugins ignition-gazebo6) + +set(CMAKE_AUTOMOC ON) +qt5_add_resources(resources_rcc src/gui/EStop.qrc) + +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( - 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) -ament_target_dependencies(gz_led_strip_manager panther_utils) + 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 + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) + +install( + TARGETS EStop + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) + +install( + TARGETS LEDStrip + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) -install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME}) 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_simulation_plugins.xml) + +ament_environment_hooks( + "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in") + ament_package() diff --git a/panther_gazebo/CONFIGURATION.md b/panther_gazebo/CONFIGURATION.md new file mode 100644 index 000000000..c41cc924c --- /dev/null +++ b/panther_gazebo/CONFIGURATION.md @@ -0,0 +1,61 @@ +# panther_gazebo + +## Use of GPS in Simulation + +The NavSat system is utilized to publish the Panther robot's position within the Gazebo world. It manages navigation satellite sensors, such as GPS, which report position and velocity in spherical coordinates (latitude/longitude) through Ignition Transport. + +The NavSat sensors requires the spherical coordinates of the world origin to be configured. This configuration can be accomplished, for instance, by employing the `` tag within the world's SDF or by utilizing the Ignition `/world/world_name/set_spherical_coordinates` service. + +To obtain GPS data in Ignition, follow these steps: + +- Include the [ANT02](https://github.com/husarion/ros_components_description/blob/ros2/urdf/teltonika_003R-00253.urdf.xacro) by adding the following lines to your [components.yaml](../panther_description/config/components.yaml) file inside the `components` list: + +```yaml + - type: ANT02 + parent_link: cover_link + xyz: 0.185 -0.12 0.0 + rpy: 0.0 0.0 3.14 + device_namespace: gps +``` + +- Add the following tag to your world's SDF file and specify this file using the `world` parameter (the default `husarion_world.sdf` file already includes this tag): + +```xml + + EARTH_WGS84 + ENU + 53.1978 + 18.3732 + 0 + 0 + +``` + +## Linear Battery Plugin + +It is possible to simulate the battery operation of the Panther robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin_config.yaml` file or in the file pointed to by the `battery_config_path` parameter. Below, you will find the plugin parameters that enable battery simulation. + +- `simulate_discharging` [*bool*, default: **false**]: Enables battery simulation. If set to `true`, the battery will discharge **at a constant rate** (regardless of joint torque), and if it depletes completely, the robot will stop moving. When set to `false`, the battery will not discharge, but the battery status information will still be published on the `battery/battery_status` topic. +- `initial_charge_percentage` [*float*, default: **70.0**]: Sets the initial charge percentage of the battery. +- `charging_time` [*float*, default: **6.0**]: Specifies the charging time for the battery in hours. +- `power_load` [*float*, default: **120.0**]: Represents the average power load during normal operation **[W]** and is initially set to 120.0 W. With the default power load of 120.0 W, the robot can operate for up to 8 hours. When the `simulate_discharging` parameter is set to `false`, this value defaults to 0.0 W. Users are encouraged to customize this value to match their specific requirements. For more information on Panther power consumption, please refer to [Panther Battery & Charging Documentation](https://husarion.com/manuals/panther/#battery--charging). + +> [!NOTE] +> +> The battery model is quite simple and involves significant simplifications. As a result, the battery discharge rate observed on the physical robot may differ from the model's predictions. + +### Charging Process + +Unfortunately, there is no straightforward way to exchange `LinearBatteryPlugin` services between ROS and Gazebo Transport, so you need to use the `ign` commands. As a result, the method of charging differs between the real and simulated robot. + +You can start the charging process by calling the Ignition service: + +```bash +ign service --service /model/panther/battery/battery/recharge/start --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 +``` + +and stop it using: + +```bash +ign service --service /model/panther/battery/battery/recharge/stop --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 +``` diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md index 1c7783994..a4d3bb85a 100644 --- a/panther_gazebo/README.md +++ b/panther_gazebo/README.md @@ -2,97 +2,74 @@ The package contains a launch file and source files used to run the robot simulation in Gazebo. The simulator tries to reproduce the behavior of a real robot as much as possible, including the provision of an analogous ROS_API. -## Available Launch File +## Launch Files -- `spawn_robot.launch.py` - is responsible for spawning the robot in the simulator -- `simulate_robot.launch.py` - is responsible for giving birth to the robot and simulating its physical behavior, such as driving, displaying data, etc. -- `simulate_multiple_robots.launch.py` - similar to the above with logic allowing you to quickly add a swarm of robots -- **`simulation.launch.py`** - a target file that runs the gazebo simulator and adds and simulates the robot's behavior in accordance with the given arguments. +- `spawn_robot.launch.py`: Responsible for spawning the robot in the simulator. +- `simulate_robot.launch.py`: Responsible for giving birth to the robot and simulating its physical behavior, such as driving, displaying data, etc. +- `simulate_multiple_robots.launch.py`: Similar to the above with logic allowing you to quickly add a swarm of robots. +- **`simulation.launch.py`**: A target file that runs the gazebo simulator that adds and simulates the robot's behavior in accordance with the given arguments. -## Usage +## Configuration Files -The recommended method for launching the simulation is by utilizing the [simulation.launch.py](https://github.com/husarion/panther_ros/panther_gazebo/launch/simulation.launch.py) file. Below, you will find launch arguments that enable simulation configuration. You can also launch more robots using `spawn.launch.py` ​​after the system has been started. +- [`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. +- [`teleop_with_estop.config`](./config/teleop_with_estop.config): Gazebo layout configuration file, which adds E-Stop and Teleop widgets. -### Launch Arguments +## ROS Nodes -- `add_world_transform` [*bool*, default: **False**]: Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots). -- `battery_config_path` [*string*, default: **panther_gazebo/config/battery_plugin_config.yaml**]: Path to the Ignition `LinearBatteryPlugin` configuration file. This configuration is intended for use in simulations only. For more information on how to configure this plugin, please refer to the [Linear Battery Plugin](#linear-battery-plugin) section. -- `controller_config_path` [*string*, default: **panther_controller/config/_controller.yaml**]: Path to the controller configuration file. If you want to use a custom configuration, you can specify the path to your custom controller configuration file here. -- `gz_bridge_config_path` [*string*, default: **panther_gazebo/config/gz_bridge.yaml**]: Path to the `parameter_bridge` configuration file. For detailed information on configuring the `parameter_bridge`, please refer to this [example](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge#example-5-configuring-the-bridge-via-yaml). -- `gz_log_level` [*[0-4]*, default: **1**]: Adjust the level of console output. -- `headless_mode` [*bool*, default: **False**]: Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the amount of calculations. -- `x` [*float*, default: **5.0**]: spawn position **[m]** of the robot in the world in **X** direction. -- `y` [*float*, default: **-5.0**]: spawn position **[m]** of the robot in the world in **Y** direction. -- `z` [*float*, default: **0.2**]: spawn position **[m]** of the robot in the world in **Z** direction. -- `roll` [*float*, default: **0.0**]: spawn roll angle **[rad]** of the robot in the world. -- `pitch` [*float*, default: **0.0**]: spawn pitch angle **[rad]** of the robot in the world. -- `yaw` [*float*, default: **0.0**]: spawn yaw angle **[rad]** of the robot in the world. -- `publish_robot_state` [*bool*, default: **true**]: Whether to launch the robot_state_publisher node. When set to `false`, users should publish their own robot description. -- `robots` [*yaml style*, default: **""**]: The list of the robots spawned in the simulation e.g. robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}'" -- `wheel_config_path` [*string*, default: **panther_description/config/.yaml**]: Path to the wheel configuration file. If you want to use a custom configuration, you can specify the path to your custom wheel configuration file here. Please refer to the `wheel_type` parameter description for more information. -- `wheel_type` [*string*, default: **WH01**]: Specify the type of wheel. If you select a value from the provided options (`WH01`, `WH02`, `WH04`), you can disregard the `wheel_config_path` and `controller_config_path` parameters. If you have custom wheels, set this parameter to `CUSTOM` and provide the necessary configurations. -- `world` [*string*, default: **-r /worlds/husarion_world.sdf**]: path to Gazebo world file used for simulation. +### EStop -### Changing Wheel Type +`EStop` is a Gazebo GUI plugin responsible for easy and convenient changing of the robot's E-stop state. -It is possible to change Panther wheels model in simulation. All you need to do is to point to new wheel and controller configuration files using `wheel_config_path` and `controller_config_path` parameters. These files should be based on the default ones, i.e., [WH01_controller.yaml](https://github.com/husarion/panther_ros/panther_controller/config/WH01_controller.yaml) and [WH01.yaml](https://github.com/husarion/panther_ros/panther_description/config/WH01.yaml). +#### Service Clients -### Linear Battery Plugin +- `hardware/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop. +- `hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop. -It is possible to simulate the battery operation of the Panther robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin_config.yaml` file or in the file pointed to by the `battery_config_path` parameter. Below, you will find the plugin parameters that enable battery simulation. +### GzPantherSystem -- `simulate_discharging` [*bool*, default: **false**]: Enables battery simulation. If set to `true`, the battery will discharge **at a constant rate** (regardless of joint torque), and if it depletes completely, the robot will stop moving. When set to `false`, the battery will not discharge, but the battery status information will still be published on the `battery/battery_status` topic. -- `initial_charge_percentage` [*float*, default: **70.0**]: Sets the initial charge percentage of the battery. -- `charging_time` [*float*, default: **6.0**]: Specifies the charging time for the battery in hours. -- `power_load` [*float*, default: **120.0**]: Represents the average power load during normal operation **[W]** and is initially set to 120.0 W. With the default power load of 120.0 W, the robot can operate for up to 8 hours. When the `simulate_discharging` parameter is set to `false`, this value defaults to 0.0 W. Users are encouraged to customize this value to match their specific requirements. For more information on Panther power consumption, please refer to [Panther Battery & Charging Documentation](https://husarion.com/manuals/panther/#battery--charging). +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. -> **Note** -> -> The `percentage` field has a range of 0.0-100.0. This differs from the real functioning of Panther, where, in accordance with the BatteryState message definition, the value is within the range of 0.0-1.0. +#### Publishers -#### Charging Process +- `gz_ros2_control/e_stop` [*std_msgs/Bool*]: Current E-stop state. -Unfortunately, there is no straightforward way to exchange `LinearBatteryPlugin` services between ROS and Gazebo Transport, so you need to use the `ign` commands. As a result, the method of charging differs between the real and simulated robot. +#### Service Servers -You can start the charging process by calling the Ignition service: +- `gz_ros2_control/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop. +- `gz_ros2_control/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop. -```bash -ign service --service /model/panther/battery/panther_battery/recharge/start --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 -``` +> [!NOTE] +> Above topics and services should be remapped to match real robot -and stop it using: +#### Parameters -```bash -ign service --service /model/panther/battery/panther_battery/recharge/stop --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 -``` +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)). -### Use of GPS in Simulation +- `e_stop_initial_state` [*bool*, default: **true**]: Initial state of E-stop. -The NavSat system is utilized to publish the Panther robot's position within the Gazebo world. It manages navigation satellite sensors, such as GPS, which report position and velocity in spherical coordinates (latitude/longitude) through Ignition Transport. +### LEDStrip -The NavSat sensors requires the spherical coordinates of the world origin to be configured. This configuration can be accomplished, for instance, by employing the `` tag within the world's SDF or by utilizing the Ignition `/world/world_name/set_spherical_coordinates` service. +`LEDStrip` is a Gazebo System plugin responsible for visualizing light and displaying markers based on the data received from a `gz::msgs::Image` message. -To obtain GPS data in Ignition, follow these steps: +> [!NOTE] +> The topics and services mentioned below are related to Gazebo interfaces, not ROS interfaces. -- Include the [ANT02](https://github.com/husarion/ros_components_description/blob/ros2/urdf/external_antenna.urdf.xacro) by adding the following lines to your [components.yaml](https://github.com/husarion/panther_ros/blob/ros2/panther_description/config/components.yaml) file inside the `components` list: +#### Subscribers -```yaml - - type: ANT02 - parent_link: cover_link - xyz: 0.185 -0.12 0.0 - rpy: 0.0 0.0 3.14 - device_namespace: gps -``` +- `{topic}` [*gz::msgs::Image*]: Subscribes to an image message for visualization. The topic is specified via a parameter. -- Add the following tag to your world's SDF file and specify this file using the `world` parameter (the default `husarion_world.sdf` file already includes this tag): +#### Service Servers -```xml - - EARTH_WGS84 - ENU - 53.1978 - 18.3732 - 0 - 0 - -``` +- `/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 5f8fa9209..d7f8114bf 100644 --- a/panther_gazebo/config/gz_bridge.yaml +++ b/panther_gazebo/config/gz_bridge.yaml @@ -18,13 +18,13 @@ direction: GZ_TO_ROS - ros_topic_name: lights/channel_1_frame - gz_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 - ros_topic_name: lights/channel_2_frame - gz_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 c5c24c254..000000000 --- a/panther_gazebo/config/led_strips.yaml +++ /dev/null @@ -1,21 +0,0 @@ -chanel_1: - frequency: 15 # 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: 15 # 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/config/teleop_with_estop.config b/panther_gazebo/config/teleop_with_estop.config new file mode 100644 index 000000000..3bdf8538a --- /dev/null +++ b/panther_gazebo/config/teleop_with_estop.config @@ -0,0 +1,1266 @@ + + + + -1 + -1 + + 1850 + 1016 +