diff --git a/.docs/panther_ros2_api.drawio.svg b/.docs/panther_ros2_api.drawio.svg deleted file mode 100644 index 29cefa8b..00000000 --- a/.docs/panther_ros2_api.drawio.svg +++ /dev/null @@ -1,4 +0,0 @@ - - - -
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/.docs/ros2_system_design.drawio.svg b/.docs/ros2_system_design.drawio.svg new file mode 100644 index 00000000..ef55b4d4 --- /dev/null +++ b/.docs/ros2_system_design.drawio.svg @@ -0,0 +1,4 @@ + + + +
husarion_ugv_localization
husarion_ugv_localization
husarion_ugv_controller
husarion_ugv_controller
husarion_ugv_lights
husarion_ugv_lights
husarion_ugv_hardware_interfaces
husarion_ugv_hardware_interfaces
husarion_ugv_battery
husarion_ugv_battery
husarion_ugv_manager
husarion_ugv_manager
husarion_ugv_diagnostics
husarion_ugv_diagnostics
{Robot}System
{Robot}System
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
PhidgetImuSensor

PhidgetImuSensor
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/robot_driver_state
hardware/robot_driver_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/workflows/release-candidate.yaml b/.github/workflows/release-candidate.yaml index c1504ad1..f2744f57 100644 --- a/.github/workflows/release-candidate.yaml +++ b/.github/workflows/release-candidate.yaml @@ -5,17 +5,31 @@ on: workflow_dispatch: inputs: version: - description: Release candidate version. IMPORTANT - required format `X.X.X`, eg `2.0.1`. + description: Release candidate version (format `X.X.X`, e.g. `2.0.1`). required: true date: - description: Release candidate date stamp. IMPORTANT - required format `YYYYMMDD`, eg `20240430`. + description: Release candidate date stamp (format `YYYYMMDD`, e.g. `20240430`). required: true env: RC_BRANCH_NAME: ${{ github.event.inputs.version }}-${{ github.event.inputs.date }} jobs: - # TODO: Add unit testing for panther_ros when ready + check_docs: + name: Check docs build + runs-on: ubuntu-22.04 + steps: + - name: Trigger repository build workflow + uses: convictional/trigger-workflow-and-wait@v1.6.1 + with: + owner: husarion + repo: docs_new + github_token: ${{ secrets.GH_PAT }} + workflow_file_name: test-release.yml + ref: master + client_payload: '{"husarion_ugv_branch": "ros2-devel"}' + + # TODO: Add unit testing for panther_ros when ready unit_test_panther_ros: name: Run unit tests for panther_ros runs-on: ubuntu-22.04 @@ -55,7 +69,6 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: update-tags-in-compose.yaml ref: ${{ env.RC_BRANCH_NAME }} - wait_interval: 10 client_payload: | { "branch_name": "${{ env.RC_BRANCH_NAME }}", @@ -76,7 +89,6 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: ros-docker-image.yaml ref: ${{ env.RC_BRANCH_NAME }} - wait_interval: 10 client_payload: | { "panther_codebase_version": "${{ env.RC_BRANCH_NAME }}", @@ -98,7 +110,6 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: build_and_deploy_image.yaml ref: ${{ env.RC_BRANCH_NAME }} - wait_interval: 10 client_payload: | { "dev_image": "true", @@ -121,7 +132,6 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: build_and_deploy_flash_os_image.yaml ref: ${{ env.RC_BRANCH_NAME }} - wait_interval: 10 client_payload: | { "image_tag": "${{ github.event.inputs.version }}" diff --git a/.github/workflows/release-project.yaml b/.github/workflows/release-project.yaml index 47800b4f..4ac40ef2 100644 --- a/.github/workflows/release-project.yaml +++ b/.github/workflows/release-project.yaml @@ -1,5 +1,5 @@ --- -name: Release Panther project +name: Release project on: workflow_dispatch: @@ -30,11 +30,11 @@ env: MAIN_BRANCH: ros2 jobs: - release_panther_msgs: - name: Release panther_msgs repository + release_project: + name: Release Husarion UGV project runs-on: ubuntu-22.04 steps: - - name: Trigger repository release workflow + - name: Release panther_msgs repository uses: convictional/trigger-workflow-and-wait@v1.6.1 with: owner: husarion @@ -42,7 +42,6 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: release-repository.yaml ref: ${{ env.RC_BRANCH_NAME }} - wait_interval: 10 client_payload: | { "release_candidate": "${{ env.RC_BRANCH_NAME }}", @@ -52,13 +51,7 @@ jobs: "prerelease": "${{ github.event.inputs.prerelease }}" } - release_panther_ros: - name: Release panther_ros repository - needs: - - release_panther_msgs - runs-on: ubuntu-22.04 - steps: - - name: Trigger panther_ros release workflow + - name: Release panther_ros repository uses: convictional/trigger-workflow-and-wait@v1.6.1 with: owner: husarion @@ -66,7 +59,6 @@ jobs: github_token: ${{ secrets.GITHUB_TOKEN }} # Use the default GITHUB_TOKEN for local repository workflow_file_name: release-repository.yaml ref: ${{ env.RC_BRANCH_NAME }} - wait_interval: 10 client_payload: | { "release_candidate": "${{ env.RC_BRANCH_NAME }}", @@ -76,13 +68,7 @@ jobs: "prerelease": "${{ github.event.inputs.prerelease }}" } - rebuild_and_push_docker_images: - name: Rebuild panther docker images with new version - runs-on: ubuntu-22.04 - needs: - - release_panther_ros - steps: - - name: Trigger repository build workflow + - name: Rebuild panther docker images with new version uses: convictional/trigger-workflow-and-wait@v1.6.1 with: owner: husarion @@ -90,7 +76,6 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: ros-docker-image.yaml ref: ${{ env.RC_BRANCH_NAME }} - wait_interval: 10 client_payload: | { "panther_codebase_version": "${{ github.event.inputs.version }}", @@ -98,13 +83,7 @@ jobs: "target_distro": "humble" } - release_panther_docker: - name: Release panther-docker repository - needs: - - rebuild_and_push_docker_images - runs-on: ubuntu-22.04 - steps: - - name: Trigger repository release workflow + - name: Release panther-docker repository uses: convictional/trigger-workflow-and-wait@v1.6.1 with: owner: husarion @@ -112,7 +91,6 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: release-repository.yaml ref: ${{ env.RC_BRANCH_NAME }} - wait_interval: 10 client_payload: | { "release_candidate": "${{ env.RC_BRANCH_NAME }}", @@ -122,14 +100,7 @@ jobs: "prerelease": "${{ github.event.inputs.prerelease }}" } - release_panther_rpi_os_image: - name: Release panther-rpi-os-img repository - if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }} - needs: - - release_panther_docker - runs-on: ubuntu-22.04 - steps: - - name: Trigger repository release workflow + - name: Release panther-rpi-os-img repository uses: convictional/trigger-workflow-and-wait@v1.6.1 with: owner: husarion @@ -137,7 +108,6 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: release-repository.yaml ref: ${{ env.RC_BRANCH_NAME }} - wait_interval: 10 client_payload: | { "release_candidate": "${{ env.RC_BRANCH_NAME }}", @@ -147,14 +117,8 @@ jobs: "prerelease": "${{ github.event.inputs.prerelease }}" } - build_and_publish_rpi_image: - name: Build panther system image - if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }} - needs: - - release_panther_rpi_os_image - runs-on: ubuntu-22.04 - steps: - - name: Trigger repository build workflow + - name: Build panther system image + if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }} uses: convictional/trigger-workflow-and-wait@v1.6.1 with: owner: husarion @@ -162,7 +126,6 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: build_and_deploy_image.yaml ref: ${{ env.MAIN_BRANCH }} - wait_interval: 10 client_payload: | { "dev_image": "false", @@ -170,14 +133,8 @@ jobs: "image_tag": "${{ github.event.inputs.version }}" } - build_and_publish_rpi_flash_os_image: - name: Build panther flash OS image - if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }} - needs: - - build_and_publish_rpi_image - runs-on: ubuntu-22.04 - steps: - - name: Trigger repository build workflow + - name: Build panther flash OS image + if: ${{ fromJSON(github.event.inputs.automatic_mode) == true }} uses: convictional/trigger-workflow-and-wait@v1.6.1 with: owner: husarion @@ -185,8 +142,16 @@ jobs: github_token: ${{ secrets.GH_PAT }} workflow_file_name: build_and_deploy_flash_os_image.yaml ref: ${{ env.MAIN_BRANCH }} - wait_interval: 10 client_payload: | { "image_tag": "${{ github.event.inputs.version }}" } + + - name: Trigger repository build workflow + uses: convictional/trigger-workflow-and-wait@v1.6.1 + with: + owner: husarion + repo: docs_new + github_token: ${{ secrets.GH_PAT }} + workflow_file_name: build.yml + ref: master diff --git a/.github/workflows/run-unit-tests.yaml b/.github/workflows/run-unit-tests.yaml index 6d7d4878..48c1ebf6 100644 --- a/.github/workflows/run-unit-tests.yaml +++ b/.github/workflows/run-unit-tests.yaml @@ -1,5 +1,5 @@ --- -name: Run panther unit tests +name: Run unit tests on: workflow_dispatch: @@ -28,12 +28,12 @@ jobs: uses: actions/checkout@v3 with: ref: ${{ github.ref }} - path: ros2_ws/src/panther_ros + path: ros2_ws/src/husarion_ugv - name: Resolve dependencies working-directory: ros2_ws run: | - vcs import < src/panther_ros/panther/panther_hardware.repos src + vcs import < src/husarion_ugv/husarion_ugv/hardware_deps.repos src sudo apt update rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y @@ -43,15 +43,15 @@ jobs: run: | source /opt/ros/$ROS_DISTRO/setup.bash if [ -f install/setup.bash ]; then source install/setup.bash; fi - colcon build --symlink-install --parallel-workers $(nproc) --packages-up-to panther --cmake-args -DCMAKE_CXX_FLAGS='-fprofile-arcs -ftest-coverage' + colcon build --symlink-install --parallel-workers $(nproc) --packages-up-to husarion_ugv --cmake-args -DCMAKE_CXX_FLAGS='-fprofile-arcs -ftest-coverage' - name: Test working-directory: ros2_ws run: | source install/setup.bash - colcon test --packages-up-to panther --retest-until-pass 10 --event-handlers console_cohesion+ --return-code-on-test-failure + colcon test --packages-up-to husarion_ugv --retest-until-pass 10 --event-handlers console_cohesion+ --return-code-on-test-failure echo "result=$?" >> ${{ runner.temp }}/${{ env.TEST_RESULT_FILENAME }} - colcon lcov-result --packages-up-to panther --verbose >> ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} + colcon lcov-result --packages-up-to husarion_ugv --verbose >> ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} lines_cov=$(cat ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} | grep -E 'lines' | head -1) functions_cov=$(cat ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} | grep -E 'functions' | head -1) branches_cov=$(cat ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} | grep -E 'branches' | head -1) diff --git a/.gitignore b/.gitignore index f42d579a..ac9f2a85 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,6 @@ -devel/ -logs/ build/ +log/ +install/ bin/ lib/ msg_gen/ @@ -13,8 +13,6 @@ msg/*Feedback.msg msg/*Goal.msg msg/*Result.msg msg/_*.py -build_isolated/ -devel_isolated/ # Generated by dynamic reconfigure *.cfgc diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c426d210..a685e4f1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -47,7 +47,7 @@ repos: entry: codespell args: [ - "--exclude-file=panther_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds", + "--exclude-file=husarion_ugv_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds", "--ignore-words-list", "ned" # north, east, down (NED) ] diff --git a/README.md b/README.md index 633e44f3..63c8c9ab 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ -# panther_ros +# husarion_ugv -ROS 2 packages for Panther autonomous mobile robot +ROS 2 packages for Husarion UGV (Unmanned Ground Vehicle). The repository is a collection of necessary packages enabling the launch of the Lynx and Panther robots. [![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit)](https://github.com/pre-commit/pre-commit) @@ -16,7 +16,7 @@ ROS 2 packages for Panther autonomous mobile robot ```bash mkdir ~/husarion_ws cd ~/husarion_ws -git clone -b ros2 https://github.com/husarion/panther_ros.git src/panther_ros +git clone -b ros2 https://github.com/husarion/panther_ros.git src/husarion_ugv ``` ### Configure environment @@ -38,7 +38,7 @@ export HUSARION_ROS_BUILD_TYPE=simulation ### Build ``` bash -vcs import src < src/panther_ros/panther/panther_$HUSARION_ROS_BUILD_TYPE.repos +vcs import src < src/husarion_ugv/husarion_ugv/${HUSARION_ROS_BUILD_TYPE}_deps.repos cp -r src/ros2_controllers/diff_drive_controller src cp -r src/ros2_controllers/imu_sensor_broadcaster src @@ -49,28 +49,31 @@ rosdep update --rosdistro $ROS_DISTRO rosdep install --from-paths src -y -i source /opt/ros/$ROS_DISTRO/setup.bash -colcon build --symlink-install --packages-up-to panther --cmake-args -DCMAKE_BUILD_TYPE=Release +colcon build --symlink-install --packages-up-to husarion_ugv --cmake-args -DCMAKE_BUILD_TYPE=Release source install/setup.bash ``` >[!NOTE] -> To build code on a real robot you need to run above commands on the Panther Built-in Computer. +> To build code on a real robot you need to run above commands on the robot Built-in Computer. ### Running Real robot: ```bash -ros2 launch panther_bringup bringup.launch.py +ros2 launch husarion_ugv_bringup bringup.launch.py ``` Simulation: ```bash -ros2 launch panther_gazebo simulation.launch.py +ros2 launch husarion_ugv_gazebo simulation.launch.py ``` +> [!IMPORTANT] +> You can change spawning robot in simulation, by adding `robot_model:={robot_model}` argument. + ### 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. @@ -80,45 +83,46 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🤖 | 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` | +| 🤖 | 🖥️ | 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` | +| ✅ | ✅ | `animations_config_path` | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
***string:*** [`{robot_model}_animations.yaml`](./husarion_ugv_lights/config) | +| ❌ | ✅ | `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 robot's URDF. Available 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`](./husarion_ugv_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`](./husarion_ugv_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` | +| ✅ | ✅ | `lights_bt_project_path` | Path to BehaviorTree project file, responsible for lights management.
***string:*** [`LightsBT.btproj`](./husarion_ugv_manager/behavior_trees/LightsBT.btproj) | +| ✅ | ✅ | `localization_config_path` | Specify the path to the localization configuration file.
***string:*** [`relative_localization.yaml`](./husarion_ugv_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 URDF of specified robot.
***bool:*** `True` | +| ❌ | ✅ | `robot_model` | Specify robot model type.
***string:*** `env(ROBOT_MODEL_NAME)` (choices: `lynx`, `panther`) | +| ✅ | ✅ | `safety_bt_project_path` | Path to BehaviorTree project file, responsible for safety and shutdown management.
***string:*** [`SafetyBT.btproj`](./husarion_ugv_manager/behavior_trees/SafetyBT.btproj) | +| ✅ | ✅ | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown.
***string:*** [`shutdown_hosts.yaml`](./husarion_ugv_manager/config/shutdown_hosts.yaml) | +| ✅ | ✅ | `use_ekf` | Enable or disable EKF.
***bool:*** `True` | +| ❌ | ✅ | `use_rviz` | Run RViz simultaneously.
***bool:*** `True` | +| ✅ | ✅ | `use_sim` | Whether simulation is used.
***bool:*** `False` | +| ✅ | ✅ | `user_led_animations_path` | 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` | Specify the wheel type. If the selected wheel type is not 'custom', the wheel_config_path and controller_config_path arguments will be automatically adjusted and can be omitted.
***string:*** `WH01` (for Panther), `WH05` (for Lynx) (choices: `WH01`, `WH02`, `WH04`, `WH05`, `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.0` | +| ❌ | ✅ | `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`) +> To read the arguments for individual packages, add the `-s` flag to the `ros2 launch` command (e.g. `ros2 launch husarion_ugv_bringup bringup.launch.py ​​-s`) ## Developer Info diff --git a/ROS_API.md b/ROS_API.md index 1a92bdc2..f29c0664 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -3,9 +3,9 @@ > [!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. +> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Lynx and 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: +> We would greatly appreciate your feedback regarding the Husarion UGV 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) @@ -13,20 +13,20 @@ ## 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. +This section describes the ROS packages used in Husarion UGV. These packages are located in the [panther_ros](https://github.com/husarion/panther_ros) GitHub repository. > [!NOTE] -> **Differences in ROS System** +> **Hardware Compatibility** > -> 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. +> This package supports **Lynx v0.2+**, **Panther v1.2+**. There may be small differences between robot models. This is caused by the 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 default way to communicate with our robots 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. +The graph below represents Husarion UVG 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) +![Husarion UVG ROS 2 System Design Diagram](.docs/ros2_system_design.drawio.svg) ## ROS Interfaces @@ -37,91 +37,92 @@ Below is information about the physical robot API. For the simulation, topics an | 🤖 | 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)* | +| 🤖 | 🖥️ | Node name | Description | +| --- | --- | ------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ✅ | ❌ | `battery_driver` | Publishes battery state read from ADC unit.
[*panther_batter/battery_driver_node*](./husarion_ugv_battery/include/husarion_ugv_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)* | +| ✅ | ❌ | `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_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)* | +| ❌ | ✅ | `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.
[husarion_ugv_gazebo/EStop](./husarion_ugv_gazebo/src/gui/e_stop.cpp) | +| ✅ | ❌ | `hardware_controller` | Plugin responsible for communicating with engine controllers via the CAN bus and providing E-Stop functionalities.
*[husarion_ugv_hardware_interfaces/{robot_model}System](./husarion_ugv_hardware_interfaces/src/robot_system/)* | +| ✅ | ✅ | `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 `light_driver` node.
[*husarion_ugv_lights/LightsControllerNode*](./husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp) | +| ✅ | ❌ | `lights_driver` | This node is responsible for displaying frames on the robot's lights.
[*husarion_ugv_lights/LightsDriverNode*](./husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp) | +| ✅ | ✅ | `lights_manager` | Node responsible for managing lights animation scheduling.
[husarion_ugv_manager/lights_manager](./husarion_ugv_manager/include/husarion_ugv_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)* | +| ✅ | ✅ | `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.
*[husarion_ugv_manager/safety_manager_node](./husarion_ugv_manager/include/husarion_ugv_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.
*[husarion_ugv_diagnostics/system_monitor_node](./husarion_ugv_diagnostics/include/husarion_ugv_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) | +| 🤖 | 🖥️ | Topic | Description | +| --- | --- | ----------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ✅ | ✅ | `battery/battery_status` | Mean values of both batteries will be published if the robot 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/robot_driver_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
[*panther_msgs/RobotDriverState*](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) | +| 🤖 | 🖥️ | 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) | +| 🤖 | 🖥️ | 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) | +| ✅ | ✅ | `localization/enable` | Enable EKF node.
[std_srvs/srv/Empty](https://docs.ros2.org/latest/api/std_srvs/srv/Empty.html) | +| ✅ | ❌ | `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/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/husarion_ugv/CHANGELOG.rst similarity index 100% rename from panther/CHANGELOG.rst rename to husarion_ugv/CHANGELOG.rst diff --git a/panther/CMakeLists.txt b/husarion_ugv/CMakeLists.txt similarity index 80% rename from panther/CMakeLists.txt rename to husarion_ugv/CMakeLists.txt index 626fc36a..2c2d0e25 100644 --- a/panther/CMakeLists.txt +++ b/husarion_ugv/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(panther) +project(husarion_ugv) find_package(ament_cmake REQUIRED) diff --git a/husarion_ugv/README.md b/husarion_ugv/README.md new file mode 100644 index 00000000..bc079c16 --- /dev/null +++ b/husarion_ugv/README.md @@ -0,0 +1,3 @@ +# husarion_ugv + +ROS 2 Metapackage composing basic functionalities of the Husarion UGV robot with VCS Tool yaml files directing to external robot dependencies. diff --git a/panther/panther_hardware.repos b/husarion_ugv/hardware_deps.repos similarity index 94% rename from panther/panther_hardware.repos rename to husarion_ugv/hardware_deps.repos index 27289d3b..eceb83d6 100644 --- a/panther/panther_hardware.repos +++ b/husarion_ugv/hardware_deps.repos @@ -10,7 +10,7 @@ repositories: panther_msgs: type: git url: https://github.com/husarion/panther_msgs.git - version: fcee4d9f249a62adc113eb80be4885b08024ee9c + version: ros2-devel ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git diff --git a/panther/package.xml b/husarion_ugv/package.xml similarity index 71% rename from panther/package.xml rename to husarion_ugv/package.xml index 788b9385..3702562a 100644 --- a/panther/package.xml +++ b/husarion_ugv/package.xml @@ -1,9 +1,9 @@ - panther + husarion_ugv 2.1.2 - Meta package that contains all packages of Panther + Meta package that contains all packages of Husarion UGV (Unmanned Ground Vehicle) Husarion Apache License 2.0 @@ -15,8 +15,8 @@ ament_cmake - panther_bringup - panther_gazebo + husarion_ugv_bringup + husarion_ugv_gazebo ament_cmake diff --git a/panther/panther_simulation.repos b/husarion_ugv/simulation_deps.repos similarity index 95% rename from panther/panther_simulation.repos rename to husarion_ugv/simulation_deps.repos index 83df44fa..e6c81113 100644 --- a/panther/panther_simulation.repos +++ b/husarion_ugv/simulation_deps.repos @@ -10,7 +10,7 @@ repositories: panther_msgs: type: git url: https://github.com/husarion/panther_msgs.git - version: fcee4d9f249a62adc113eb80be4885b08024ee9c + version: ros2-devel ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git diff --git a/panther_battery/CHANGELOG.rst b/husarion_ugv_battery/CHANGELOG.rst similarity index 100% rename from panther_battery/CHANGELOG.rst rename to husarion_ugv_battery/CHANGELOG.rst diff --git a/panther_battery/CMakeLists.txt b/husarion_ugv_battery/CMakeLists.txt similarity index 84% rename from panther_battery/CMakeLists.txt rename to husarion_ugv_battery/CMakeLists.txt index 4e12bab8..a3dc0c8d 100644 --- a/panther_battery/CMakeLists.txt +++ b/husarion_ugv_battery/CMakeLists.txt @@ -1,18 +1,24 @@ cmake_minimum_required(VERSION 3.10.2) -project(panther_battery) +project(husarion_ugv_battery) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) +set(PACKAGE_DEPENDENCIES + ament_cmake + diagnostic_updater + generate_parameter_library + panther_msgs + husarion_ugv_utils + rclcpp + sensor_msgs) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) find_package(${PACKAGE} REQUIRED) endforeach() -include_directories(include ${panther_utils_INCLUDE_DIRS}) +include_directories(include ${husarion_ugv_utils_INCLUDE_DIRS}) add_executable( battery_driver_node @@ -25,6 +31,9 @@ add_executable( src/battery_publisher/single_battery_publisher.cpp) ament_target_dependencies(battery_driver_node ${PACKAGE_DEPENDENCIES}) +generate_parameter_library(battery_parameters config/battery_parameters.yaml) +target_link_libraries(battery_driver_node battery_parameters) + install(TARGETS battery_driver_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) @@ -76,6 +85,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_battery_publisher ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_battery_publisher + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_single_battery_publisher @@ -89,6 +100,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_single_battery_publisher ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_single_battery_publisher + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_dual_battery_publisher @@ -102,6 +115,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_dual_battery_publisher ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_dual_battery_publisher + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_battery_driver_node_adc_dual @@ -118,6 +133,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_dual ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_adc_dual + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_battery_driver_node_adc_single @@ -134,6 +151,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_single ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_adc_single + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_battery_driver_node_roboteq @@ -150,6 +169,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_roboteq ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_roboteq + battery_parameters) endif() diff --git a/panther_battery/README.md b/husarion_ugv_battery/README.md similarity index 55% rename from panther_battery/README.md rename to husarion_ugv_battery/README.md index dc151104..263ea252 100644 --- a/panther_battery/README.md +++ b/husarion_ugv_battery/README.md @@ -1,6 +1,6 @@ -# panther_battery +# husarion_ugv_battery -The package containing nodes monitoring and publishing the internal battery state of the Husarion Panther robot. +The package containing nodes monitoring and publishing the internal battery state of the Husarion UGV. ## Launch Files @@ -8,32 +8,36 @@ This package contains: - `battery.launch.py`: Responsible for activating battery node, which dealing with reading and publishing battery data. +## Configuration Files + +- [`battery_parameters.yaml`](./config/battery_parameters.yaml): Defines parameters for `battery_driver_node`. + ## ROS Nodes ### battery_driver_node -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 battery state read from ADC unit. #### Publishes - `_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/battery_status` [*sensor_msgs/BatteryState*]: Mean values of both batteries if robot 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. #### Subscribers - `hardware/io_state` [*panther_msgs/IOState*]: Current state of IO. -- `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data. +- `hardware/robot_driver_state` [*panther_msgs/RobotDriverState*]: Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data. #### Parameters -- `~/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. +- `~/adc.device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC number 0 IIO device. +- `~/adc.device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC number 1 IIO device. +- `~/adc.ma_window_len.charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings. +- `~/adc.ma_window_len.temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings. - `~/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. +- `~/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 (deprecated). diff --git a/husarion_ugv_battery/config/battery_parameters.yaml b/husarion_ugv_battery/config/battery_parameters.yaml new file mode 100644 index 00000000..ab09541c --- /dev/null +++ b/husarion_ugv_battery/config/battery_parameters.yaml @@ -0,0 +1,52 @@ +battery: + adc: + device0: + type: string + default_value: "/sys/bus/iio/devices/iio:device0" + description: "Internal ADC0 IIO device." + validation: { not_empty<> } + + device1: + type: string + default_value: "/sys/bus/iio/devices/iio:device1" + description: "Internal ADC1 IIO device." + validation: { not_empty<> } + + ma_window_len: + charge: + type: int + default_value: 10 + description: "Window length of a moving average, used to smooth out battery charge readings." + validation: { gt<>: 0 } + + temp: + type: int + default_value: 10 + description: "Window length of a moving average, used to smooth out battery temperature readings." + validation: { gt<>: 0 } + + ma_window_len: + voltage: + type: int + default_value: 10 + description: "Window length of a moving average, used to smooth out battery voltage readings." + validation: { gt<>: 0 } + + current: + type: int + default_value: 10 + description: "Window length of a moving average, used to smooth out battery current readings." + validation: { gt<>: 0 } + + roboteq: + driver_state_timeout: + type: double + default_value: 0.2 + description: "Timeout in seconds after which driver state messages will be considered old. Used as a fallback when ADC data is not available." + validation: { gt<>: 0.0 } + + battery_timeout: + type: double + default_value: 1.0 + description: "Timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state." + validation: { gt<>: 0.0 } diff --git a/panther_battery/include/panther_battery/adc_data_reader.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/adc_data_reader.hpp similarity index 90% rename from panther_battery/include/panther_battery/adc_data_reader.hpp rename to husarion_ugv_battery/include/husarion_ugv_battery/adc_data_reader.hpp index 08dca0d0..59e1fb57 100644 --- a/panther_battery/include/panther_battery/adc_data_reader.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_battery/adc_data_reader.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_ADC_DATA_READER_HPP_ -#define PANTHER_BATTERY_ADC_DATA_READER_HPP_ +#ifndef HUSARION_UGV_BATTERY_ADC_DATA_READER_HPP_ +#define HUSARION_UGV_BATTERY_ADC_DATA_READER_HPP_ #include #include #include #include -namespace panther_battery +namespace husarion_ugv_battery { class ADCDataReader @@ -75,6 +75,6 @@ class ADCDataReader const std::filesystem::path device_path_; }; -} // namespace panther_battery +} // namespace husarion_ugv_battery -#endif // PANTHER_BATTERY_ADC_DATA_READER_HPP_ +#endif // HUSARION_UGV_BATTERY_ADC_DATA_READER_HPP_ diff --git a/panther_battery/include/panther_battery/battery/adc_battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp similarity index 76% rename from panther_battery/include/panther_battery/battery/adc_battery.hpp rename to husarion_ugv_battery/include/husarion_ugv_battery/battery/adc_battery.hpp index 6aebfa09..20b39591 100644 --- a/panther_battery/include/panther_battery/battery/adc_battery.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_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_BATTERY_ADC_BATTERY_HPP_ -#define PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_ +#ifndef HUSARION_UGV_BATTERY_BATTERY_ADC_BATTERY_HPP_ +#define HUSARION_UGV_BATTERY_BATTERY_ADC_BATTERY_HPP_ #include #include @@ -21,10 +21,10 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery/battery.hpp" -#include "panther_utils/moving_average.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_utils/moving_average.hpp" -namespace panther_battery +namespace husarion_ugv_battery { using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -77,7 +77,12 @@ class ADCBattery : public Battery static constexpr float kR1 = 10000.0; static constexpr float kR0 = 10000.0; static constexpr float kUSupply = 3.28; - static constexpr float kKelvinToCelciusOffset = 273.15; + static constexpr float kKelvinToCelsiusOffset = 273.15; + + // Threshold used for diagnostics. At this voltage level, and below, the battery shall draw a + // significant current from the charger if the charger is connected. If not, the charging circuit + // may be broken. + static constexpr float kBatteryCCCheckTresh = 41.2; float voltage_raw_; float current_raw_; @@ -89,12 +94,12 @@ class ADCBattery : public Battery const std::function ReadTemp; const std::function ReadCharge; - std::unique_ptr> voltage_ma_; - std::unique_ptr> current_ma_; - std::unique_ptr> temp_ma_; - std::unique_ptr> charge_ma_; + std::unique_ptr> voltage_ma_; + std::unique_ptr> current_ma_; + std::unique_ptr> temp_ma_; + std::unique_ptr> charge_ma_; }; -} // namespace panther_battery +} // namespace husarion_ugv_battery -#endif // PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_ +#endif // HUSARION_UGV_BATTERY_BATTERY_ADC_BATTERY_HPP_ diff --git a/panther_battery/include/panther_battery/battery/battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp similarity index 86% rename from panther_battery/include/panther_battery/battery/battery.hpp rename to husarion_ugv_battery/include/husarion_ugv_battery/battery/battery.hpp index eb067aa6..94392164 100644 --- a/panther_battery/include/panther_battery/battery/battery.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_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_BATTERY_HPP_ -#define PANTHER_BATTERY_BATTERY_BATTERY_HPP_ +#ifndef HUSARION_UGV_BATTERY_BATTERY_BATTERY_HPP_ +#define HUSARION_UGV_BATTERY_BATTERY_BATTERY_HPP_ #include #include @@ -27,7 +27,7 @@ #include "panther_msgs/msg/charging_status.hpp" -namespace panther_battery +namespace husarion_ugv_battery { using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -57,14 +57,14 @@ class Battery float GetBatteryPercent(const float voltage) const { - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 5; 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); + (battery_approx_a_values[5] * voltage + battery_approx_b_values[5]) / 100, 0.0f, 1.0f); } void ResetBatteryMsgs(const rclcpp::Time & header_stamp) @@ -117,9 +117,10 @@ class Battery 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}; + static constexpr float battery_approx_ranges[5] = {41.25, 37.0, 36.0, 35.0, 33.7}; + static constexpr float battery_approx_a_values[6] = {8.665, 9.153, 19.8, 22.84, 10.538, 0.989}; + static constexpr float battery_approx_b_values[6] = {-258.73, -278.861, -672.6, + -782.04, -351.47, -29.669}; std::string error_msg_; BatteryStateMsg battery_state_; @@ -127,6 +128,6 @@ class Battery ChargingStatusMsg charging_status_; }; -} // namespace panther_battery +} // namespace husarion_ugv_battery -#endif // PANTHER_BATTERY_BATTERY_BATTERY_HPP_ +#endif // HUSARION_UGV_BATTERY_BATTERY_BATTERY_HPP_ diff --git a/panther_battery/include/panther_battery/battery/roboteq_battery.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery/roboteq_battery.hpp similarity index 61% rename from panther_battery/include/panther_battery/battery/roboteq_battery.hpp rename to husarion_ugv_battery/include/husarion_ugv_battery/battery/roboteq_battery.hpp index a0019fc5..0b785f79 100644 --- a/panther_battery/include/panther_battery/battery/roboteq_battery.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_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_BATTERY_ROBOTEQ_BATTERY_HPP_ -#define PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ +#ifndef HUSARION_UGV_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ +#define HUSARION_UGV_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ #include #include @@ -21,15 +21,17 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_msgs/msg/driver_state.hpp" +#include "panther_msgs/msg/driver_state_named.hpp" +#include "panther_msgs/msg/robot_driver_state.hpp" -#include "panther_battery/battery/battery.hpp" -#include "panther_utils/moving_average.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_utils/moving_average.hpp" -namespace panther_battery +namespace husarion_ugv_battery { -using DriverStateMsg = panther_msgs::msg::DriverState; +using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; +using DriverStateNamedMsg = panther_msgs::msg::DriverStateNamed; struct RoboteqBatteryParams { @@ -42,7 +44,7 @@ class RoboteqBattery : public Battery { public: RoboteqBattery( - const std::function & get_driver_state, + const std::function & get_driver_state, const RoboteqBatteryParams & params); ~RoboteqBattery() {} @@ -54,7 +56,7 @@ class RoboteqBattery : public Battery float GetLoadCurrent() override { return std::numeric_limits::quiet_NaN(); } protected: - void ValidateDriverStateMsg(const rclcpp::Time & header_stamp); + void ValidateRobotDriverStateMsg(const rclcpp::Time & header_stamp); private: void UpdateBatteryMsgs(const rclcpp::Time & header_stamp); @@ -62,18 +64,19 @@ class RoboteqBattery : public Battery void UpdateBatteryStateRaw(); void UpdateChargingStatus(const rclcpp::Time & header_stamp); std::uint8_t GetBatteryHealth(const float voltage); + bool DriverStateHeartbeatTimeout(); - std::function GetDriverState; + std::function GetRobotDriverState; const float driver_state_timeout_; float voltage_raw_; float current_raw_; - DriverStateMsg::SharedPtr driver_state_; + RobotDriverStateMsg::SharedPtr driver_state_; - std::unique_ptr> voltage_ma_; - std::unique_ptr> current_ma_; + std::unique_ptr> voltage_ma_; + std::unique_ptr> current_ma_; }; -} // namespace panther_battery +} // namespace husarion_ugv_battery -#endif // PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ +#endif // HUSARION_UGV_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ diff --git a/panther_battery/include/panther_battery/battery_driver_node.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp similarity index 65% rename from panther_battery/include/panther_battery/battery_driver_node.hpp rename to husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp index c13371c9..ca113540 100644 --- a/panther_battery/include/panther_battery/battery_driver_node.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_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_DRIVER_NODE_HPP_ -#define PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_ +#ifndef HUSARION_UGV_BATTERY_BATTERY_DRIVER_NODE_HPP_ +#define HUSARION_UGV_BATTERY_BATTERY_DRIVER_NODE_HPP_ #include #include @@ -21,16 +21,18 @@ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" -#include "panther_msgs/msg/driver_state.hpp" +#include "panther_msgs/msg/robot_driver_state.hpp" -#include "panther_battery/adc_data_reader.hpp" -#include "panther_battery/battery/battery.hpp" -#include "panther_battery/battery_publisher/battery_publisher.hpp" +#include "battery_parameters.hpp" -namespace panther_battery +#include "husarion_ugv_battery/adc_data_reader.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp" + +namespace husarion_ugv_battery { -using DriverStateMsg = panther_msgs::msg::DriverState; +using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; class BatteryDriverNode : public rclcpp::Node { @@ -46,7 +48,7 @@ class BatteryDriverNode : public rclcpp::Node static constexpr int kADCCurrentOffset = 625; - DriverStateMsg::SharedPtr driver_state_; + RobotDriverStateMsg::SharedPtr driver_state_; std::shared_ptr adc0_reader_; std::shared_ptr adc1_reader_; @@ -54,12 +56,15 @@ class BatteryDriverNode : public rclcpp::Node std::shared_ptr battery_2_; std::shared_ptr battery_publisher_; - rclcpp::Subscription::SharedPtr driver_state_sub_; + std::shared_ptr param_listener_; + battery::Params params_; + + rclcpp::Subscription::SharedPtr driver_state_sub_; rclcpp::TimerBase::SharedPtr battery_pub_timer_; std::shared_ptr diagnostic_updater_; }; -} // namespace panther_battery +} // namespace husarion_ugv_battery -#endif // PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_ +#endif // HUSARION_UGV_BATTERY_BATTERY_DRIVER_NODE_HPP_ diff --git a/panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp similarity index 86% rename from panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp rename to husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp index 688f1899..29e52609 100644 --- a/panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_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_BATTERY_PUBLISHER_HPP_ -#define PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ +#ifndef HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ +#define HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ #include @@ -25,7 +25,7 @@ #include "panther_msgs/msg/charging_status.hpp" #include "panther_msgs/msg/io_state.hpp" -namespace panther_battery +namespace husarion_ugv_battery { using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -37,7 +37,8 @@ class BatteryPublisher public: BatteryPublisher( const rclcpp::Node::SharedPtr & node, - const std::shared_ptr & diagnostic_updater); + const std::shared_ptr & diagnostic_updater, + const double battery_timeout); ~BatteryPublisher() {} @@ -71,6 +72,6 @@ class BatteryPublisher rclcpp::Subscription::SharedPtr io_state_sub_; }; -} // namespace panther_battery +} // namespace husarion_ugv_battery -#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ +#endif // HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ diff --git a/panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp similarity index 79% rename from panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp rename to husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp index 9890465f..4200c621 100644 --- a/panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ -#define PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ +#ifndef HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ +#define HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ #include #include #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery/battery.hpp" -#include "panther_battery/battery_publisher/battery_publisher.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp" -namespace panther_battery +namespace husarion_ugv_battery { class DualBatteryPublisher : public BatteryPublisher @@ -32,7 +32,8 @@ class DualBatteryPublisher : public BatteryPublisher DualBatteryPublisher( const rclcpp::Node::SharedPtr & node, const std::shared_ptr & diagnostic_updater, - const std::shared_ptr & battery_1, const std::shared_ptr & battery_2); + const double battery_timeout, const std::shared_ptr & battery_1, + const std::shared_ptr & battery_2); ~DualBatteryPublisher() {} @@ -66,6 +67,6 @@ class DualBatteryPublisher : public BatteryPublisher rclcpp::Publisher::SharedPtr charging_status_pub_; }; -} // namespace panther_battery +} // namespace husarion_ugv_battery -#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ +#endif // HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ diff --git a/panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp similarity index 74% rename from panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp rename to husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp index 29be9bc5..1b68e2e9 100644 --- a/panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ -#define PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ +#ifndef HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ +#define HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ #include #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery/battery.hpp" -#include "panther_battery/battery_publisher/battery_publisher.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp" -namespace panther_battery +namespace husarion_ugv_battery { class SingleBatteryPublisher : public BatteryPublisher @@ -31,7 +31,7 @@ class SingleBatteryPublisher : public BatteryPublisher SingleBatteryPublisher( const rclcpp::Node::SharedPtr & node, const std::shared_ptr & diagnostic_updater, - const std::shared_ptr & battery); + const double battery_timeout, const std::shared_ptr & battery); ~SingleBatteryPublisher() {} @@ -51,6 +51,6 @@ class SingleBatteryPublisher : public BatteryPublisher rclcpp::Publisher::SharedPtr charging_status_pub_; }; -} // namespace panther_battery +} // namespace husarion_ugv_battery -#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ +#endif // HUSARION_UGV_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ diff --git a/panther_battery/launch/battery.launch.py b/husarion_ugv_battery/launch/battery.launch.py similarity index 88% rename from panther_battery/launch/battery.launch.py rename to husarion_ugv_battery/launch/battery.launch.py index 3719d587..d8327e33 100644 --- a/panther_battery/launch/battery.launch.py +++ b/husarion_ugv_battery/launch/battery.launch.py @@ -28,13 +28,10 @@ def generate_launch_description(): description="Add namespace to all launched nodes.", ) - panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0") - battery_driver_node = Node( - package="panther_battery", + package="husarion_ugv_battery", executable="battery_driver_node", name="battery_driver", - parameters=[{"panther_version": panther_version}], namespace=namespace, remappings=[("/diagnostics", "diagnostics")], emulate_tty=True, diff --git a/panther_battery/package.xml b/husarion_ugv_battery/package.xml similarity index 89% rename from panther_battery/package.xml rename to husarion_ugv_battery/package.xml index 53e7fab8..2f77ad8f 100644 --- a/panther_battery/package.xml +++ b/husarion_ugv_battery/package.xml @@ -1,7 +1,7 @@ - panther_battery + husarion_ugv_battery 2.1.2 Nodes monitoring the battery state of Husarion Panhter robot Husarion @@ -17,8 +17,9 @@ ament_cmake diagnostic_updater + generate_parameter_library + husarion_ugv_utils panther_msgs - panther_utils rclcpp sensor_msgs diff --git a/panther_battery/src/battery/adc_battery.cpp b/husarion_ugv_battery/src/battery/adc_battery.cpp similarity index 91% rename from panther_battery/src/battery/adc_battery.cpp rename to husarion_ugv_battery/src/battery/adc_battery.cpp index 968f4823..68db4d73 100644 --- a/panther_battery/src/battery/adc_battery.cpp +++ b/husarion_ugv_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/battery/adc_battery.hpp" +#include "husarion_ugv_battery/battery/adc_battery.hpp" #include #include @@ -24,9 +24,9 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_utils/moving_average.hpp" +#include "husarion_ugv_utils/moving_average.hpp" -namespace panther_battery +namespace husarion_ugv_battery { ADCBattery::ADCBattery( @@ -35,13 +35,13 @@ ADCBattery::ADCBattery( const ADCBatteryParams & params) : ReadVoltage(read_voltage), ReadCurrent(read_current), ReadTemp(read_temp), ReadCharge(read_charge) { - voltage_ma_ = std::make_unique>( + voltage_ma_ = std::make_unique>( params.voltage_window_len, std::numeric_limits::quiet_NaN()); - current_ma_ = std::make_unique>( + current_ma_ = std::make_unique>( params.current_window_len, std::numeric_limits::quiet_NaN()); - temp_ma_ = std::make_unique>( + temp_ma_ = std::make_unique>( params.temp_window_len, std::numeric_limits::quiet_NaN()); - charge_ma_ = std::make_unique>( + charge_ma_ = std::make_unique>( params.charge_window_len, std::numeric_limits::quiet_NaN()); } @@ -107,7 +107,7 @@ float ADCBattery::ADCToBatteryTemp(const float adc_data) const const float R_therm = (adc_data * kR1) / (kUSupply - adc_data); return (kTempCoeffA * kTempCoeffB / (kTempCoeffA * logf(R_therm / kR0) + kTempCoeffB)) - - kKelvinToCelciusOffset; + kKelvinToCelsiusOffset; } void ADCBattery::UpdateBatteryMsgs(const rclcpp::Time & header_stamp, const bool charger_connected) @@ -167,7 +167,7 @@ std::uint8_t ADCBattery::GetBatteryStatus(const float charge, const bool charger if (charger_connected) { if (fabs(battery_state_.percentage - 1.0f) < std::numeric_limits::epsilon()) { return BatteryStateMsg::POWER_SUPPLY_STATUS_FULL; - } else if (charge > kChargingCurrentTresh) { + } else if (charge > kChargingCurrentTresh || battery_state_.voltage > kBatteryCCCheckTresh) { return BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING; } else { return BatteryStateMsg::POWER_SUPPLY_STATUS_NOT_CHARGING; @@ -197,4 +197,4 @@ std::uint8_t ADCBattery::GetBatteryHealth(const float voltage, const float temp) } } -} // namespace panther_battery +} // namespace husarion_ugv_battery diff --git a/panther_battery/src/battery/roboteq_battery.cpp b/husarion_ugv_battery/src/battery/roboteq_battery.cpp similarity index 76% rename from panther_battery/src/battery/roboteq_battery.cpp rename to husarion_ugv_battery/src/battery/roboteq_battery.cpp index 6653208e..7d2d870d 100644 --- a/panther_battery/src/battery/roboteq_battery.cpp +++ b/husarion_ugv_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/battery/roboteq_battery.hpp" +#include "husarion_ugv_battery/battery/roboteq_battery.hpp" #include #include @@ -23,19 +23,19 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_utils/moving_average.hpp" +#include "husarion_ugv_utils/moving_average.hpp" -namespace panther_battery +namespace husarion_ugv_battery { RoboteqBattery::RoboteqBattery( - const std::function & get_driver_state, + const std::function & get_driver_state, const RoboteqBatteryParams & params) -: GetDriverState(get_driver_state), driver_state_timeout_(params.driver_state_timeout) +: GetRobotDriverState(get_driver_state), driver_state_timeout_(params.driver_state_timeout) { - voltage_ma_ = std::make_unique>( + voltage_ma_ = std::make_unique>( params.voltage_window_len, std::numeric_limits::quiet_NaN()); - current_ma_ = std::make_unique>( + current_ma_ = std::make_unique>( params.current_window_len, std::numeric_limits::quiet_NaN()); } @@ -43,11 +43,20 @@ bool RoboteqBattery::Present() { return true; } void RoboteqBattery::Update(const rclcpp::Time & header_stamp, const bool /* charger_connected */) { - driver_state_ = GetDriverState(); - ValidateDriverStateMsg(header_stamp); - - voltage_raw_ = (driver_state_->front.voltage + driver_state_->rear.voltage) / 2.0f; - current_raw_ = driver_state_->front.current + driver_state_->rear.current; + driver_state_ = GetRobotDriverState(); + ValidateRobotDriverStateMsg(header_stamp); + + float voltage = 0.0f; + float current = 0.0f; + std::for_each( + driver_state_->driver_states.begin(), driver_state_->driver_states.end(), + [&voltage, ¤t](const DriverStateNamedMsg & driver) { + voltage += driver.state.voltage; + current += driver.state.current; + }); + + voltage_raw_ = voltage / driver_state_->driver_states.size(); + current_raw_ = current; voltage_ma_->Roll(voltage_raw_); current_ma_->Roll(current_raw_); @@ -63,7 +72,7 @@ void RoboteqBattery::Reset(const rclcpp::Time & header_stamp) SetErrorMsg(""); } -void RoboteqBattery::ValidateDriverStateMsg(const rclcpp::Time & header_stamp) +void RoboteqBattery::ValidateRobotDriverStateMsg(const rclcpp::Time & header_stamp) { if (!driver_state_) { throw std::runtime_error("Waiting for driver state message to arrive."); @@ -74,7 +83,7 @@ void RoboteqBattery::ValidateDriverStateMsg(const rclcpp::Time & header_stamp) throw std::runtime_error("Driver state message timeout."); } - if (driver_state_->front.heartbeat_timeout || driver_state_->rear.heartbeat_timeout) { + if (DriverStateHeartbeatTimeout()) { throw std::runtime_error("Motor controller heartbeat timeout error."); } } @@ -141,4 +150,11 @@ std::uint8_t RoboteqBattery::GetBatteryHealth(const float voltage) } } -} // namespace panther_battery +bool RoboteqBattery::DriverStateHeartbeatTimeout() +{ + return std::any_of( + driver_state_->driver_states.begin(), driver_state_->driver_states.end(), + [](const DriverStateNamedMsg & driver) { return driver.state.heartbeat_timeout; }); +} + +} // namespace husarion_ugv_battery diff --git a/panther_battery/src/battery_driver_node.cpp b/husarion_ugv_battery/src/battery_driver_node.cpp similarity index 58% rename from panther_battery/src/battery_driver_node.cpp rename to husarion_ugv_battery/src/battery_driver_node.cpp index d1ff8acd..db6a063b 100644 --- a/panther_battery/src/battery_driver_node.cpp +++ b/husarion_ugv_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_driver_node.hpp" +#include "husarion_ugv_battery/battery_driver_node.hpp" #include #include @@ -24,15 +24,15 @@ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" -#include "panther_battery/adc_data_reader.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" +#include "husarion_ugv_battery/adc_data_reader.hpp" +#include "husarion_ugv_battery/battery/adc_battery.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_battery/battery/roboteq_battery.hpp" +#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp" +#include "husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp" +#include "husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp" -namespace panther_battery +namespace husarion_ugv_battery { BatteryDriverNode::BatteryDriverNode( @@ -41,9 +41,9 @@ BatteryDriverNode::BatteryDriverNode( { RCLCPP_INFO(this->get_logger(), "Constructing node."); - this->declare_parameter("panther_version", 1.2); - this->declare_parameter("ma_window_len/voltage", 10); - this->declare_parameter("ma_window_len/current", 10); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); // Running at 10 Hz battery_pub_timer_ = this->create_wall_timer( @@ -58,17 +58,14 @@ void BatteryDriverNode::Initialize() { RCLCPP_INFO(this->get_logger(), "Initializing."); - const float panther_version = this->get_parameter("panther_version").as_double(); - if (panther_version >= (1.2f - std::numeric_limits::epsilon())) { - try { - InitializeWithADCBattery(); - return; - } catch (const std::runtime_error & e) { - RCLCPP_WARN_STREAM( - this->get_logger(), "An exception occurred while initializing with ADC: " - << e.what() - << " Falling back to using Roboteq drivers to publish battery data."); - } + try { + InitializeWithADCBattery(); + return; + } catch (const std::runtime_error & e) { + RCLCPP_WARN_STREAM( + this->get_logger(), "An exception occurred while initializing with ADC: " + << e.what() + << " Falling back to using Roboteq drivers to publish battery data."); } InitializeWithRoboteqBattery(); @@ -79,22 +76,17 @@ void BatteryDriverNode::InitializeWithADCBattery() { RCLCPP_DEBUG(this->get_logger(), "Initializing with ADC data."); - this->declare_parameter("adc/device0", "/sys/bus/iio/devices/iio:device0"); - this->declare_parameter("adc/device1", "/sys/bus/iio/devices/iio:device1"); - this->declare_parameter("adc/ma_window_len/temp", 10); - this->declare_parameter("adc/ma_window_len/charge", 10); - - const std::string adc0_device_path = this->get_parameter("adc/device0").as_string(); - const std::string adc1_device_path = this->get_parameter("adc/device1").as_string(); + const std::string adc0_device_path = this->params_.adc.device0; + const std::string adc1_device_path = this->params_.adc.device1; adc0_reader_ = std::make_shared(adc0_device_path); adc1_reader_ = std::make_shared(adc1_device_path); const ADCBatteryParams battery_params = { - static_cast(this->get_parameter("ma_window_len/voltage").as_int()), - static_cast(this->get_parameter("ma_window_len/current").as_int()), - static_cast(this->get_parameter("adc/ma_window_len/temp").as_int()), - static_cast(this->get_parameter("adc/ma_window_len/charge").as_int()), + static_cast(this->params_.ma_window_len.voltage), + static_cast(this->params_.ma_window_len.current), + static_cast(this->params_.adc.ma_window_len.temp), + static_cast(this->params_.adc.ma_window_len.charge), }; battery_2_ = std::make_shared( @@ -110,7 +102,8 @@ void BatteryDriverNode::InitializeWithADCBattery() std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 1, 0), std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 3, 0), battery_params); battery_publisher_ = std::make_shared( - this->shared_from_this(), diagnostic_updater_, battery_1_, battery_2_); + this->shared_from_this(), diagnostic_updater_, params_.battery_timeout, battery_1_, + battery_2_); } else { battery_2_.reset(); battery_1_ = std::make_shared( @@ -122,7 +115,7 @@ void BatteryDriverNode::InitializeWithADCBattery() std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 1, 0), std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 3, 0), battery_params); battery_publisher_ = std::make_shared( - this->shared_from_this(), diagnostic_updater_, battery_1_); + this->shared_from_this(), diagnostic_updater_, params_.battery_timeout, battery_1_); } RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data."); @@ -132,22 +125,20 @@ void BatteryDriverNode::InitializeWithRoboteqBattery() { RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data."); - this->declare_parameter("roboteq/driver_state_timeout", 0.2); - const RoboteqBatteryParams battery_params = { - static_cast(this->get_parameter("roboteq/driver_state_timeout").as_double()), - static_cast(this->get_parameter("ma_window_len/voltage").as_int()), - static_cast(this->get_parameter("ma_window_len/current").as_int()), + static_cast(this->params_.roboteq.driver_state_timeout), + static_cast(this->params_.ma_window_len.voltage), + static_cast(this->params_.ma_window_len.current), }; - driver_state_sub_ = this->create_subscription( - "hardware/motor_controllers_state", 5, - [&](const DriverStateMsg::SharedPtr msg) { driver_state_ = msg; }); + driver_state_sub_ = this->create_subscription( + "hardware/robot_driver_state", 5, + [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_ = msg; }); battery_1_ = std::make_shared([&]() { return driver_state_; }, battery_params); battery_publisher_ = std::make_shared( - this->shared_from_this(), diagnostic_updater_, battery_1_); + this->shared_from_this(), diagnostic_updater_, this->params_.battery_timeout, battery_1_); RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data."); } @@ -161,4 +152,4 @@ void BatteryDriverNode::BatteryPubTimerCB() battery_publisher_->Publish(); } -} // namespace panther_battery +} // namespace husarion_ugv_battery diff --git a/panther_battery/src/battery_publisher/battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp similarity index 90% rename from panther_battery/src/battery_publisher/battery_publisher.cpp rename to husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp index e3ccb14b..4f2e9c71 100644 --- a/panther_battery/src/battery_publisher/battery_publisher.cpp +++ b/husarion_ugv_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/battery_publisher.hpp" +#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp" #include #include @@ -21,17 +21,17 @@ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" -namespace panther_battery +namespace husarion_ugv_battery { BatteryPublisher::BatteryPublisher( const rclcpp::Node::SharedPtr & node, - const std::shared_ptr & diagnostic_updater) -: node_(std::move(node)), diagnostic_updater_(std::move(diagnostic_updater)) + const std::shared_ptr & diagnostic_updater, + const double battery_timeout) +: node_(std::move(node)), + diagnostic_updater_(std::move(diagnostic_updater)), + battery_timeout_(battery_timeout) { - node->declare_parameter("battery_timeout", 1.0); - battery_timeout_ = node->get_parameter("battery_timeout").as_double(); - charger_connected_ = false; last_battery_info_time_ = rclcpp::Time(std::int64_t(0), RCL_ROS_TIME); @@ -49,12 +49,12 @@ void BatteryPublisher::Publish() this->Update(); last_battery_info_time_ = GetClock()->now(); } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM_THROTTLE( + RCLCPP_WARN_STREAM_THROTTLE( GetLogger(), *GetClock(), 1000, "An exception occurred while reading battery data: " << e.what()); diagnostic_updater_->broadcast( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Error reading battery data: " + std::string(e.what())); } @@ -140,4 +140,4 @@ rclcpp::Clock::SharedPtr BatteryPublisher::GetClock() return std::make_shared(RCL_ROS_TIME); } -} // namespace panther_battery +} // namespace husarion_ugv_battery diff --git a/panther_battery/src/battery_publisher/dual_battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp similarity index 94% rename from panther_battery/src/battery_publisher/dual_battery_publisher.cpp rename to husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp index fba8198d..1e345280 100644 --- a/panther_battery/src/battery_publisher/dual_battery_publisher.cpp +++ b/husarion_ugv_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/battery_publisher/dual_battery_publisher.hpp" +#include "husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp" #include #include @@ -24,18 +24,19 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_battery/battery/battery.hpp" -#include "panther_battery/battery_publisher/battery_publisher.hpp" -#include "panther_utils/ros_utils.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp" +#include "husarion_ugv_utils/ros_utils.hpp" -namespace panther_battery +namespace husarion_ugv_battery { DualBatteryPublisher::DualBatteryPublisher( const rclcpp::Node::SharedPtr & node, const std::shared_ptr & diagnostic_updater, - const std::shared_ptr & battery_1, const std::shared_ptr & battery_2) -: BatteryPublisher(std::move(node), std::move(diagnostic_updater)), + const double battery_timeout, const std::shared_ptr & battery_1, + const std::shared_ptr & battery_2) +: BatteryPublisher(std::move(node), std::move(diagnostic_updater), battery_timeout), battery_1_(std::move(battery_1)), battery_2_(std::move(battery_2)) { @@ -172,10 +173,10 @@ ChargingStatusMsg DualBatteryPublisher::MergeChargingStatusMsgs( ChargingStatusMsg charging_status_msg; try { - panther_utils::ros::VerifyTimestampGap( + husarion_ugv_utils::ros::VerifyTimestampGap( charging_status_msg_1.header, charging_status_msg_2.header, std::chrono::seconds(1)); - charging_status_msg.header = panther_utils::ros::MergeHeaders( + charging_status_msg.header = husarion_ugv_utils::ros::MergeHeaders( charging_status_msg_1.header, charging_status_msg_2.header); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM_THROTTLE( @@ -251,4 +252,4 @@ void DualBatteryPublisher::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWr status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Battery status monitoring"); } -} // namespace panther_battery +} // namespace husarion_ugv_battery diff --git a/panther_battery/src/battery_publisher/single_battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp similarity index 89% rename from panther_battery/src/battery_publisher/single_battery_publisher.cpp rename to husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp index 4916e647..4fff8f68 100644 --- a/panther_battery/src/battery_publisher/single_battery_publisher.cpp +++ b/husarion_ugv_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/battery_publisher/single_battery_publisher.hpp" +#include "husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp" #include #include @@ -23,17 +23,18 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_battery/battery/battery.hpp" -#include "panther_battery/battery_publisher/battery_publisher.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp" -namespace panther_battery +namespace husarion_ugv_battery { SingleBatteryPublisher::SingleBatteryPublisher( const rclcpp::Node::SharedPtr & node, const std::shared_ptr & diagnostic_updater, - const std::shared_ptr & battery) -: BatteryPublisher(std::move(node), std::move(diagnostic_updater)), battery_(std::move(battery)) + const double battery_timeout, const std::shared_ptr & battery) +: BatteryPublisher(std::move(node), std::move(diagnostic_updater), battery_timeout), + battery_(std::move(battery)) { battery_pub_ = node->create_publisher("battery/battery_status", 5); battery_1_pub_ = node->create_publisher("_battery/battery_1_status_raw", 5); @@ -106,4 +107,4 @@ void SingleBatteryPublisher::DiagnoseStatus(diagnostic_updater::DiagnosticStatus status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Battery status monitoring"); } -} // namespace panther_battery +} // namespace husarion_ugv_battery diff --git a/panther_battery/src/main.cpp b/husarion_ugv_battery/src/main.cpp similarity index 86% rename from panther_battery/src/main.cpp rename to husarion_ugv_battery/src/main.cpp index b759fd33..9dc84ddf 100644 --- a/panther_battery/src/main.cpp +++ b/husarion_ugv_battery/src/main.cpp @@ -18,13 +18,14 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery_driver_node.hpp" +#include "husarion_ugv_battery/battery_driver_node.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto battery_driver_node = std::make_shared("battery_driver"); + auto battery_driver_node = + std::make_shared("battery_driver"); try { rclcpp::spin(battery_driver_node); diff --git a/panther_battery/test/battery/test_adc_battery.cpp b/husarion_ugv_battery/test/battery/test_adc_battery.cpp similarity index 91% rename from panther_battery/test/battery/test_adc_battery.cpp rename to husarion_ugv_battery/test/battery/test_adc_battery.cpp index 986f7cdb..13a5680f 100644 --- a/panther_battery/test/battery/test_adc_battery.cpp +++ b/husarion_ugv_battery/test/battery/test_adc_battery.cpp @@ -24,8 +24,8 @@ #include "panther_msgs/msg/charging_status.hpp" -#include "panther_battery/battery/adc_battery.hpp" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_battery/battery/adc_battery.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; using ChargingStatusMsg = panther_msgs::msg::ChargingStatus; @@ -54,7 +54,7 @@ class TestADCBattery : public testing::Test float battery_temp_raw_; float battery_charge_raw_; - std::unique_ptr battery_; + std::unique_ptr battery_; BatteryStateMsg battery_state_; ChargingStatusMsg charging_status_; @@ -62,8 +62,8 @@ class TestADCBattery : public testing::Test TestADCBattery::TestADCBattery() { - panther_battery::ADCBatteryParams params = {10, 10, 10, 10}; - battery_ = std::make_unique( + husarion_ugv_battery::ADCBatteryParams params = {10, 10, 10, 10}; + battery_ = std::make_unique( [&]() { return battery_voltage_raw_; }, [&]() { return battery_current_raw_; }, [&]() { return battery_temp_raw_; }, [&]() { return battery_charge_raw_; }, params); } @@ -92,8 +92,9 @@ void TestADCBattery::TestDefaultBatteryStateMsg( EXPECT_TRUE(std::isnan(battery_state_.capacity)); EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity); EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); + EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); + EXPECT_TRUE( + husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); EXPECT_TRUE(battery_state_.present); EXPECT_EQ("user_compartment", battery_state_.location); @@ -116,8 +117,9 @@ void TestADCBattery::TestBatteryStateMsg( EXPECT_TRUE(std::isnan(battery_state_.capacity)); EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity); EXPECT_FLOAT_EQ(expected_temp, battery_state_.temperature); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); + EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); + EXPECT_TRUE( + husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology); EXPECT_EQ("user_compartment", battery_state_.location); @@ -251,6 +253,11 @@ TEST_F(TestADCBattery, BatteryMsgStatusCharging) UpdateBattery(1.5, 0.01, 0.98, 0.5, true); EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING, battery_state_.power_supply_status); + + // Small charge, but voltage over 41.2 V + UpdateBattery(1.65, 0.01, 0.98, 0.04, true); + + EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING, battery_state_.power_supply_status); } TEST_F(TestADCBattery, BatteryMsgStatusNotCharging) diff --git a/panther_battery/test/battery/test_battery.cpp b/husarion_ugv_battery/test/battery/test_battery.cpp similarity index 86% rename from panther_battery/test/battery/test_battery.cpp rename to husarion_ugv_battery/test/battery/test_battery.cpp index cdbdf381..62195ca3 100644 --- a/panther_battery/test/battery/test_battery.cpp +++ b/husarion_ugv_battery/test/battery/test_battery.cpp @@ -24,13 +24,13 @@ #include "panther_msgs/msg/charging_status.hpp" -#include "panther_battery/battery/battery.hpp" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; using ChargingStatusMsg = panther_msgs::msg::ChargingStatus; -class BatteryWrapper : public panther_battery::Battery +class BatteryWrapper : public husarion_ugv_battery::Battery { public: BatteryWrapper() {} @@ -80,8 +80,9 @@ void TestBattery::TestDefaultBatteryStateMsg( EXPECT_TRUE(std::isnan(battery_state_.capacity)); EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity); EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); + EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); + EXPECT_TRUE( + husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); EXPECT_TRUE(battery_state_.present); EXPECT_EQ("user_compartment", battery_state_.location); @@ -108,10 +109,13 @@ TEST_F(TestBattery, GetErrorMsg) TEST_F(TestBattery, GetBatteryPercent) { - EXPECT_FLOAT_EQ(0.0, battery_->GetBatteryPercent(30.0f)); - 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_NEAR(0.0, battery_->GetBatteryPercent(30.0f), 1e-5); + EXPECT_NEAR(0.01979, battery_->GetBatteryPercent(32.0f), 1e-6); + EXPECT_NEAR(0.06822, battery_->GetBatteryPercent(34.0f), 1e-6); + EXPECT_NEAR(0.2878, battery_->GetBatteryPercent(35.5f), 1e-6); + EXPECT_NEAR(0.501, battery_->GetBatteryPercent(36.5f), 1e-6); + EXPECT_NEAR(0.68953, battery_->GetBatteryPercent(38.0f), 1e-6); + EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(41.4f)); EXPECT_FLOAT_EQ(1.0, battery_->GetBatteryPercent(45.0f)); } diff --git a/panther_battery/test/battery/test_roboteq_battery.cpp b/husarion_ugv_battery/test/battery/test_roboteq_battery.cpp similarity index 73% rename from panther_battery/test/battery/test_roboteq_battery.cpp rename to husarion_ugv_battery/test/battery/test_roboteq_battery.cpp index b7059e07..26674df8 100644 --- a/panther_battery/test/battery/test_roboteq_battery.cpp +++ b/husarion_ugv_battery/test/battery/test_roboteq_battery.cpp @@ -24,27 +24,28 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_msgs/msg/driver_state.hpp" +#include "panther_msgs/msg/driver_state_named.hpp" +#include "panther_msgs/msg/robot_driver_state.hpp" -#include "panther_battery/battery/roboteq_battery.hpp" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_battery/battery/roboteq_battery.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; -using DriverStateMsg = panther_msgs::msg::DriverState; +using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; -class RoboteqBatteryWrapper : public panther_battery::RoboteqBattery +class RoboteqBatteryWrapper : public husarion_ugv_battery::RoboteqBattery { public: RoboteqBatteryWrapper( - const std::function & get_driver_state, - const panther_battery::RoboteqBatteryParams & params) + const std::function & get_driver_state, + const husarion_ugv_battery::RoboteqBatteryParams & params) : RoboteqBattery(get_driver_state, params) { } - void ValidateDriverStateMsg(const rclcpp::Time & header_stamp) + void ValidateRobotDriverStateMsg(const rclcpp::Time & header_stamp) { - return RoboteqBattery::ValidateDriverStateMsg(header_stamp); + return RoboteqBattery::ValidateRobotDriverStateMsg(header_stamp); } }; @@ -63,32 +64,34 @@ class TestRoboteqBattery : public testing::Test const float expected_voltage, const float expected_current, const float expected_percentage, const std::uint8_t power_supply_status, const std::uint8_t power_supply_health); - static constexpr float kDriverStateTimeout = 0.2; + static constexpr float kRobotDriverStateTimeout = 0.2; std::unique_ptr battery_; BatteryStateMsg battery_state_; - DriverStateMsg::SharedPtr driver_state_; + RobotDriverStateMsg::SharedPtr driver_state_; }; TestRoboteqBattery::TestRoboteqBattery() { - const panther_battery::RoboteqBatteryParams params = {kDriverStateTimeout, 10, 10}; + const husarion_ugv_battery::RoboteqBatteryParams params = {kRobotDriverStateTimeout, 10, 10}; battery_ = std::make_unique([&]() { return driver_state_; }, params); } void TestRoboteqBattery::UpdateBattery(const float voltage, const float current) { if (!driver_state_) { - driver_state_ = std::make_shared(); + driver_state_ = std::make_shared(); + driver_state_->driver_states.push_back(panther_msgs::msg::DriverStateNamed()); + driver_state_->driver_states.push_back(panther_msgs::msg::DriverStateNamed()); } auto stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); driver_state_->header.stamp = stamp; - driver_state_->front.voltage = voltage; - driver_state_->rear.voltage = voltage; - driver_state_->front.current = current; - driver_state_->rear.current = current; + driver_state_->driver_states.at(0).state.voltage = voltage; + driver_state_->driver_states.at(1).state.voltage = voltage; + driver_state_->driver_states.at(0).state.current = current; + driver_state_->driver_states.at(1).state.current = current; battery_->Update(stamp, false); battery_state_ = battery_->GetBatteryMsg(); @@ -102,8 +105,8 @@ void TestRoboteqBattery::TestDefaultBatteryStateMsg( EXPECT_TRUE(std::isnan(battery_state_.capacity)); EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity); EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); + EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); + EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); EXPECT_TRUE(battery_state_.present); EXPECT_EQ("user_compartment", battery_state_.location); @@ -125,8 +128,8 @@ void TestRoboteqBattery::TestBatteryStateMsg( EXPECT_TRUE(std::isnan(battery_state_.capacity)); EXPECT_TRUE(std::isnan(battery_state_.temperature)); EXPECT_FLOAT_EQ(20.0, battery_state_.design_capacity); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); + EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_voltage)); + EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(battery_state_.cell_temperature)); EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_TECHNOLOGY_LION, battery_state_.power_supply_technology); EXPECT_EQ("user_compartment", battery_state_.location); @@ -220,31 +223,31 @@ TEST_F(TestRoboteqBattery, GetErrorMsg) EXPECT_NE("", battery_->GetErrorMsg()); } -TEST_F(TestRoboteqBattery, ValidateDriverStateMsg) +TEST_F(TestRoboteqBattery, ValidateRobotDriverStateMsg) { auto stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); // Check empty driver_state msg - EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error); + EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error); UpdateBattery(35.0f, 0.1f); - EXPECT_NO_THROW(battery_->ValidateDriverStateMsg(stamp)); + EXPECT_NO_THROW(battery_->ValidateRobotDriverStateMsg(stamp)); // Check timeout - const std::uint32_t timeout_nanoseconds = (kDriverStateTimeout + 0.05) * 1e9; + const std::uint32_t timeout_nanoseconds = (kRobotDriverStateTimeout + 0.05) * 1e9; stamp = rclcpp::Time(0, timeout_nanoseconds, RCL_ROS_TIME); - EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error); + EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error); // Reset error stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); - EXPECT_NO_THROW(battery_->ValidateDriverStateMsg(stamp)); + EXPECT_NO_THROW(battery_->ValidateRobotDriverStateMsg(stamp)); // Check heartbeat timeout error throw - driver_state_->front.heartbeat_timeout = true; - EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error); - driver_state_->front.heartbeat_timeout = false; - driver_state_->rear.heartbeat_timeout = true; - EXPECT_THROW(battery_->ValidateDriverStateMsg(stamp), std::runtime_error); + driver_state_->driver_states.at(0).state.heartbeat_timeout = true; + EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error); + driver_state_->driver_states.at(0).state.heartbeat_timeout = false; + driver_state_->driver_states.at(1).state.heartbeat_timeout = true; + EXPECT_THROW(battery_->ValidateRobotDriverStateMsg(stamp), std::runtime_error); } int main(int argc, char ** argv) diff --git a/panther_battery/test/battery_publisher/test_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp similarity index 92% rename from panther_battery/test/battery_publisher/test_battery_publisher.cpp rename to husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp index 4ed567b1..ff2fe64a 100644 --- a/panther_battery/test/battery_publisher/test_battery_publisher.cpp +++ b/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp @@ -24,18 +24,18 @@ #include "panther_msgs/msg/io_state.hpp" -#include "panther_battery/battery_publisher/battery_publisher.hpp" +#include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; using IOStateMsg = panther_msgs::msg::IOState; -class BatteryPublisherWrapper : public panther_battery::BatteryPublisher +class BatteryPublisherWrapper : public husarion_ugv_battery::BatteryPublisher { public: BatteryPublisherWrapper( const rclcpp::Node::SharedPtr & node, std::shared_ptr diagnostic_updater) - : panther_battery::BatteryPublisher(node, diagnostic_updater) + : husarion_ugv_battery::BatteryPublisher(node, diagnostic_updater, kBatteryTimeout) { } @@ -62,6 +62,9 @@ class BatteryPublisherWrapper : public panther_battery::BatteryPublisher { status.summary(0, ""); // Avoid unused parameter compiler warning }; + +protected: + static constexpr double kBatteryTimeout = 0.2; }; class TestBatteryPublisher : public testing::Test diff --git a/panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp similarity index 92% rename from panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp rename to husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp index 0740b7fa..b14ecf63 100644 --- a/panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp +++ b/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp @@ -24,23 +24,23 @@ #include "panther_msgs/msg/charging_status.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" +#include "husarion_ugv_battery/battery/adc_battery.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp" +#include "husarion_ugv_utils/test/ros_test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; using ChargingStatusMsg = panther_msgs::msg::ChargingStatus; -class DualBatteryPublisherWrapper : public panther_battery::DualBatteryPublisher +class DualBatteryPublisherWrapper : public husarion_ugv_battery::DualBatteryPublisher { public: DualBatteryPublisherWrapper( const rclcpp::Node::SharedPtr & node, std::shared_ptr diagnostic_updater, - std::shared_ptr & battery_1, - std::shared_ptr & battery_2) - : DualBatteryPublisher(node, diagnostic_updater, battery_1, battery_2) + std::shared_ptr & battery_1, + std::shared_ptr & battery_2) + : DualBatteryPublisher(node, diagnostic_updater, kBatteryTimeout, battery_1, battery_2) { } @@ -71,6 +71,9 @@ class DualBatteryPublisherWrapper : public panther_battery::DualBatteryPublisher return DualBatteryPublisher::MergeChargingStatusMsgs( charging_status_msg_1, charging_status_msg_2); } + +private: + static constexpr double kBatteryTimeout = 0.2; }; class TestDualBatteryPublisher : public testing::Test @@ -90,8 +93,8 @@ class TestDualBatteryPublisher : public testing::Test rclcpp::Subscription::SharedPtr battery_1_sub_; rclcpp::Subscription::SharedPtr battery_2_sub_; - std::shared_ptr battery_1_; - std::shared_ptr battery_2_; + std::shared_ptr battery_1_; + std::shared_ptr battery_2_; std::shared_ptr battery_publisher_; BatteryStateMsg::SharedPtr battery_state_; BatteryStateMsg::SharedPtr battery_1_state_; @@ -100,11 +103,11 @@ class TestDualBatteryPublisher : public testing::Test TestDualBatteryPublisher::TestDualBatteryPublisher() { - panther_battery::ADCBatteryParams params = {10, 10, 10, 10}; - battery_1_ = std::make_shared( + husarion_ugv_battery::ADCBatteryParams params = {10, 10, 10, 10}; + battery_1_ = std::make_shared( [&]() { return 1.6; }, [&]() { return 0.02; }, [&]() { return 1.6; }, [&]() { return 0.4; }, params); - battery_2_ = std::make_shared( + battery_2_ = std::make_shared( [&]() { return 1.6; }, [&]() { return 0.02; }, [&]() { return 1.6; }, [&]() { return 0.4; }, params); @@ -139,13 +142,13 @@ void TestDualBatteryPublisher::TestMergeBatteryPowerSupplyStatus( TEST_F(TestDualBatteryPublisher, CorrectTopicPublished) { battery_publisher_->Publish(); - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(node_, battery_state_, std::chrono::milliseconds(1000))); + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( + node_, battery_state_, std::chrono::milliseconds(1000))); battery_publisher_->Publish(); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( node_, battery_1_state_, std::chrono::milliseconds(1000))); battery_publisher_->Publish(); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( node_, battery_2_state_, std::chrono::milliseconds(1000))); } diff --git a/panther_battery/test/battery_publisher/test_single_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp similarity index 72% rename from panther_battery/test/battery_publisher/test_single_battery_publisher.cpp rename to husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp index f7a04c3f..2afd2a37 100644 --- a/panther_battery/test/battery_publisher/test_single_battery_publisher.cpp +++ b/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp @@ -21,10 +21,10 @@ #include "sensor_msgs/msg/battery_state.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" +#include "husarion_ugv_battery/battery/adc_battery.hpp" +#include "husarion_ugv_battery/battery/battery.hpp" +#include "husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp" +#include "husarion_ugv_utils/test/ros_test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -40,16 +40,18 @@ class TestSingleBatteryPublisher : public testing::Test rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr battery_1_sub_; - std::shared_ptr battery_; - std::shared_ptr battery_publisher_; + std::shared_ptr battery_; + std::shared_ptr battery_publisher_; BatteryStateMsg::SharedPtr battery_state_; BatteryStateMsg::SharedPtr battery_1_state_; + + static constexpr double kBatteryTimeout = 0.2f; }; TestSingleBatteryPublisher::TestSingleBatteryPublisher() { - panther_battery::ADCBatteryParams params = {10, 10, 10, 10}; - battery_ = std::make_shared( + husarion_ugv_battery::ADCBatteryParams params = {10, 10, 10, 10}; + battery_ = std::make_shared( [&]() { return 1.6; }, [&]() { return 0.02; }, [&]() { return 1.6; }, [&]() { return 0.4; }, params); @@ -61,17 +63,17 @@ TestSingleBatteryPublisher::TestSingleBatteryPublisher() battery_1_sub_ = node_->create_subscription( "/_battery/battery_1_status_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; }); - battery_publisher_ = std::make_shared( - node_, diagnostic_updater_, battery_); + battery_publisher_ = std::make_shared( + node_, diagnostic_updater_, kBatteryTimeout, battery_); } TEST_F(TestSingleBatteryPublisher, CorrectTopicPublished) { battery_publisher_->Publish(); - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(node_, battery_state_, std::chrono::milliseconds(1000))); + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( + node_, battery_state_, std::chrono::milliseconds(1000))); battery_publisher_->Publish(); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( node_, battery_1_state_, std::chrono::milliseconds(1000))); } diff --git a/panther_battery/test/test_adc_data_reader.cpp b/husarion_ugv_battery/test/test_adc_data_reader.cpp similarity index 92% rename from panther_battery/test/test_adc_data_reader.cpp rename to husarion_ugv_battery/test/test_adc_data_reader.cpp index c9fd3d94..bbb322bc 100644 --- a/panther_battery/test/test_adc_data_reader.cpp +++ b/husarion_ugv_battery/test/test_adc_data_reader.cpp @@ -18,7 +18,7 @@ #include "gtest/gtest.h" -#include "panther_battery/adc_data_reader.hpp" +#include "husarion_ugv_battery/adc_data_reader.hpp" class TestADCDataReader : public testing::Test { @@ -27,7 +27,7 @@ class TestADCDataReader : public testing::Test ~TestADCDataReader(); protected: - std::shared_ptr data_reader_; + std::shared_ptr data_reader_; std::filesystem::path data_file_path_; std::filesystem::path scale_file_path_; std::ofstream file_; @@ -44,7 +44,7 @@ TestADCDataReader::TestADCDataReader() WriteNumberToFile(1.0, scale_file_path_); - data_reader_ = std::make_shared(current_path); + data_reader_ = std::make_shared(current_path); } TestADCDataReader::~TestADCDataReader() diff --git a/panther_battery/test/test_battery_driver_node_adc_dual.cpp b/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp similarity index 88% rename from panther_battery/test/test_battery_driver_node_adc_dual.cpp rename to husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp index 414fce6b..167035ce 100644 --- a/panther_battery/test/test_battery_driver_node_adc_dual.cpp +++ b/husarion_ugv_battery/test/test_battery_driver_node_adc_dual.cpp @@ -20,17 +20,17 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_utils/test/ros_test_utils.hpp" +#include "husarion_ugv_utils/test/ros_test_utils.hpp" class TestBatteryNodeADCDual : public TestBatteryNode { public: - TestBatteryNodeADCDual() : TestBatteryNode(1.2, true) {} + TestBatteryNodeADCDual() : TestBatteryNode(true, true) {} }; TEST_F(TestBatteryNodeADCDual, BatteryValues) { - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( 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 @@ -38,8 +38,8 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues) 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.21579468, battery_state_->percentage); - EXPECT_FLOAT_EQ(8.6317873, battery_state_->charge); + EXPECT_FLOAT_EQ(0.18720642, battery_state_->percentage); + EXPECT_FLOAT_EQ(7.4882565, 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); @@ -51,7 +51,7 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues) // Change value of battery 2 reading one by one and check if corresponding values in battery 1 // stops matching WriteNumberToFile(1600, std::filesystem::path(device1_path_ / "in_voltage3_raw")); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000))); EXPECT_FALSE( fabs(battery_1_state_->voltage - battery_2_state_->voltage) < @@ -67,7 +67,7 @@ TEST_F(TestBatteryNodeADCDual, 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( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000))); EXPECT_FALSE( fabs(battery_1_state_->current - battery_2_state_->current) < @@ -75,7 +75,7 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues) EXPECT_FLOAT_EQ(battery_1_state_->temperature, battery_2_state_->temperature); WriteNumberToFile(1000, std::filesystem::path(device0_path_ / "in_voltage0_raw")); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000))); EXPECT_FALSE( fabs(battery_1_state_->temperature - battery_2_state_->temperature) < @@ -84,7 +84,7 @@ TEST_F(TestBatteryNodeADCDual, BatteryValues) TEST_F(TestBatteryNodeADCDual, BatteryTimeout) { - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg should have some values @@ -98,7 +98,7 @@ TEST_F(TestBatteryNodeADCDual, BatteryTimeout) std::filesystem::remove(std::filesystem::path(device0_path_ / "in_voltage2_raw")); 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( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg values should be NaN @@ -114,14 +114,14 @@ TEST_F(TestBatteryNodeADCDual, BatteryTimeout) TEST_F(TestBatteryNodeADCDual, BatteryCharging) { // Wait for node to initialize - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( 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( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( 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_driver_node_adc_single.cpp b/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp similarity index 87% rename from panther_battery/test/test_battery_driver_node_adc_single.cpp rename to husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp index 6ddd48b3..8ad638e2 100644 --- a/panther_battery/test/test_battery_driver_node_adc_single.cpp +++ b/husarion_ugv_battery/test/test_battery_driver_node_adc_single.cpp @@ -20,12 +20,12 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_utils/test/ros_test_utils.hpp" +#include "husarion_ugv_utils/test/ros_test_utils.hpp" class TestBatteryNodeADCSingle : public TestBatteryNode { public: - TestBatteryNodeADCSingle() : TestBatteryNode(1.2, false) {} + TestBatteryNodeADCSingle() : TestBatteryNode(true, false) {} }; TEST_F(TestBatteryNodeADCSingle, BatteryValues) @@ -35,7 +35,7 @@ TEST_F(TestBatteryNodeADCSingle, BatteryValues) WriteNumberToFile(1600, std::filesystem::path(device1_path_ / "in_voltage3_raw")); WriteNumberToFile(100, std::filesystem::path(device0_path_ / "in_voltage2_raw")); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( 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 @@ -43,8 +43,8 @@ TEST_F(TestBatteryNodeADCSingle, BatteryValues) 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.21579468, battery_state_->percentage); - EXPECT_FLOAT_EQ(4.3158937, battery_state_->charge); + EXPECT_FLOAT_EQ(0.18720642, battery_state_->percentage); + EXPECT_FLOAT_EQ(3.7441282, 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); @@ -56,7 +56,7 @@ TEST_F(TestBatteryNodeADCSingle, BatteryValues) TEST_F(TestBatteryNodeADCSingle, BatteryTimeout) { - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg should have some values @@ -70,7 +70,7 @@ TEST_F(TestBatteryNodeADCSingle, BatteryTimeout) std::filesystem::remove(std::filesystem::path(device0_path_ / "in_voltage2_raw")); 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( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg values should be NaN @@ -86,14 +86,14 @@ TEST_F(TestBatteryNodeADCSingle, BatteryTimeout) TEST_F(TestBatteryNodeADCSingle, BatteryCharging) { // Wait for node to initialize - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( 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( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); EXPECT_NE(battery_state_->power_supply_status, BatteryStateMsg::POWER_SUPPLY_STATUS_DISCHARGING); @@ -105,18 +105,18 @@ TEST_F(TestBatteryNodeADCSingle, RoboteqInitOnADCFail) std::filesystem::remove_all(device0_path_); // Wait for node to initialize - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( 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; + RobotDriverStateMsg driver_state; driver_state.header.stamp = battery_driver_node_->get_clock()->now(); driver_state_pub_->publish(driver_state); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state status should be different than UNKNOWN diff --git a/panther_battery/test/test_battery_driver_node_roboteq.cpp b/husarion_ugv_battery/test/test_battery_driver_node_roboteq.cpp similarity index 80% rename from panther_battery/test/test_battery_driver_node_roboteq.cpp rename to husarion_ugv_battery/test/test_battery_driver_node_roboteq.cpp index c4cf8f9c..8393b1b9 100644 --- a/panther_battery/test/test_battery_driver_node_roboteq.cpp +++ b/husarion_ugv_battery/test/test_battery_driver_node_roboteq.cpp @@ -22,18 +22,18 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_utils/test/ros_test_utils.hpp" +#include "husarion_ugv_utils/test/ros_test_utils.hpp" class TestBatteryNodeRoboteq : public TestBatteryNode { public: - TestBatteryNodeRoboteq() : TestBatteryNode(1.0) {} + TestBatteryNodeRoboteq() : TestBatteryNode(false) {} }; TEST_F(TestBatteryNodeRoboteq, BatteryValues) { // Wait for node to initialize - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg values should be NaN @@ -44,15 +44,17 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues) EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN, battery_state_->power_supply_status); EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health); - DriverStateMsg driver_state; + panther_msgs::msg::DriverStateNamed motor_controller; + motor_controller.state.voltage = 35.0f; + motor_controller.state.current = 0.1f; + + RobotDriverStateMsg driver_state; 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; - driver_state.rear.current = 0.1f; + driver_state.driver_states.push_back(motor_controller); + driver_state.driver_states.push_back(motor_controller); driver_state_pub_->publish(driver_state); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // This is done to check if values were read correctly, not to verify calculations. @@ -70,7 +72,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues) TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) { // Wait for node to initialize - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg values should be NaN @@ -82,15 +84,17 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health); // Publish some values - DriverStateMsg driver_state; + panther_msgs::msg::DriverStateNamed motor_controller; + motor_controller.state.voltage = 35.0f; + motor_controller.state.current = 0.1f; + + RobotDriverStateMsg driver_state; 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; - driver_state.rear.current = 0.1f; + driver_state.driver_states.push_back(motor_controller); + driver_state.driver_states.push_back(motor_controller); driver_state_pub_->publish(driver_state); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg should have some values @@ -101,7 +105,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) // Wait for timeout std::this_thread::sleep_for(std::chrono::milliseconds(1500)); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + ASSERT_TRUE(husarion_ugv_utils::test_utils::WaitForMsg( battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg values should be NaN diff --git a/panther_battery/test/utils/test_battery_driver_node.hpp b/husarion_ugv_battery/test/utils/test_battery_driver_node.hpp similarity index 81% rename from panther_battery/test/utils/test_battery_driver_node.hpp rename to husarion_ugv_battery/test/utils/test_battery_driver_node.hpp index 9b2385b2..7bb73c3a 100644 --- a/panther_battery/test/utils/test_battery_driver_node.hpp +++ b/husarion_ugv_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_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ -#define PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ +#ifndef HUSARION_UGV_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ +#define HUSARION_UGV_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ #include #include @@ -28,19 +28,19 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_msgs/msg/driver_state.hpp" #include "panther_msgs/msg/io_state.hpp" +#include "panther_msgs/msg/robot_driver_state.hpp" -#include "panther_battery/battery_driver_node.hpp" +#include "husarion_ugv_battery/battery_driver_node.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; -using DriverStateMsg = panther_msgs::msg::DriverState; +using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; using IOStateMsg = panther_msgs::msg::IOState; class TestBatteryNode : public testing::Test { public: - TestBatteryNode(const float panther_version = 1.2, const bool dual_battery = false); + TestBatteryNode(const bool use_adc_battery = true, const bool dual_battery = false); ~TestBatteryNode(); protected: @@ -55,26 +55,24 @@ class TestBatteryNode : public testing::Test BatteryStateMsg::SharedPtr battery_state_; BatteryStateMsg::SharedPtr battery_1_state_; BatteryStateMsg::SharedPtr battery_2_state_; - std::shared_ptr battery_driver_node_; + std::shared_ptr battery_driver_node_; rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr battery_1_sub_; rclcpp::Subscription::SharedPtr battery_2_sub_; rclcpp::Publisher::SharedPtr io_state_pub_; - rclcpp::Publisher::SharedPtr driver_state_pub_; + rclcpp::Publisher::SharedPtr driver_state_pub_; }; -TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_battery) +TestBatteryNode::TestBatteryNode(const bool use_adc_battery, const bool dual_battery) { std::vector params; - params.push_back(rclcpp::Parameter("panther_version", panther_version)); - if (panther_version >= 1.2 - std::numeric_limits::epsilon()) { + if (use_adc_battery) { device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0; device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1; - params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string())); - params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string())); - params.push_back(rclcpp::Parameter("adc/path", testing::TempDir())); + params.push_back(rclcpp::Parameter("adc.device0", device0_path_.string())); + params.push_back(rclcpp::Parameter("adc.device1", device1_path_.string())); // Create the device0 and device1 directories if they do not exist std::filesystem::create_directory(device0_path_); @@ -104,7 +102,7 @@ TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_ba rclcpp::NodeOptions options; options.parameter_overrides(params); - battery_driver_node_ = std::make_shared( + battery_driver_node_ = std::make_shared( "battery_driver", options); battery_sub_ = battery_driver_node_->create_subscription( @@ -119,8 +117,8 @@ TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_ba io_state_pub_ = battery_driver_node_->create_publisher( "hardware/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - driver_state_pub_ = battery_driver_node_->create_publisher( - "hardware/motor_controllers_state", 10); + driver_state_pub_ = battery_driver_node_->create_publisher( + "hardware/robot_driver_state", 10); } TestBatteryNode::~TestBatteryNode() @@ -146,4 +144,4 @@ void TestBatteryNode::WriteNumberToFile(const T number, const std::string & file } } -#endif // PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ +#endif // HUSARION_UGV_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ diff --git a/panther_bringup/CHANGELOG.rst b/husarion_ugv_bringup/CHANGELOG.rst similarity index 100% rename from panther_bringup/CHANGELOG.rst rename to husarion_ugv_bringup/CHANGELOG.rst diff --git a/panther_bringup/CMakeLists.txt b/husarion_ugv_bringup/CMakeLists.txt similarity index 83% rename from panther_bringup/CMakeLists.txt rename to husarion_ugv_bringup/CMakeLists.txt index 79305750..6fb4694d 100644 --- a/panther_bringup/CMakeLists.txt +++ b/husarion_ugv_bringup/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(panther_bringup) +project(husarion_ugv_bringup) find_package(ament_cmake REQUIRED) diff --git a/panther_bringup/README.md b/husarion_ugv_bringup/README.md similarity index 87% rename from panther_bringup/README.md rename to husarion_ugv_bringup/README.md index b12b4218..3615bce3 100644 --- a/panther_bringup/README.md +++ b/husarion_ugv_bringup/README.md @@ -1,6 +1,6 @@ -# panther_bringup +# husarion_ugv_bringup -The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot. +The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion UGV. ## Launch Files diff --git a/panther_bringup/launch/bringup.launch.py b/husarion_ugv_bringup/launch/bringup.launch.py similarity index 74% rename from panther_bringup/launch/bringup.launch.py rename to husarion_ugv_bringup/launch/bringup.launch.py index 96fc0445..1e542143 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/husarion_ugv_bringup/launch/bringup.launch.py @@ -14,9 +14,11 @@ # See the License for the specific language governing permissions and # limitations under the License. + +from husarion_ugv_utils.messages import welcome_msg from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( EnvironmentVariable, @@ -24,7 +26,6 @@ PathJoinSubstitution, ) from launch_ros.substitutions import FindPackageShare -from panther_utils.welcomeMsg import welcomeMsg def generate_launch_description(): @@ -43,22 +44,15 @@ def generate_launch_description(): description="Add namespace to all launched nodes.", ) - use_ekf = LaunchConfiguration("use_ekf") - declare_use_ekf_arg = DeclareLaunchArgument( - "use_ekf", - default_value="True", - description="Enable or disable EKF.", - choices=["True", "true", "False", "false"], - ) - - serial_no = EnvironmentVariable(name="PANTHER_SERIAL_NO", default_value="----") - panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0") - welcome_msg = welcomeMsg(serial_no, panther_version) + robot_model_name = EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther") + robot_serial_no = EnvironmentVariable(name="ROBOT_SERIAL_NO", default_value="----") + robot_version = EnvironmentVariable(name="ROBOT_VERSION", default_value="1.0") + welcome_info = welcome_msg(robot_model_name, robot_serial_no, robot_version) controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_controller"), "launch", "controller.launch.py"] + [FindPackageShare("husarion_ugv_controller"), "launch", "controller.launch.py"] ) ), launch_arguments={"namespace": namespace}.items(), @@ -68,7 +62,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("panther_diagnostics"), + FindPackageShare("husarion_ugv_diagnostics"), "launch", "system_monitor.launch.py", ] @@ -80,7 +74,7 @@ def generate_launch_description(): lights_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_lights"), "launch", "lights.launch.py"] + [FindPackageShare("husarion_ugv_lights"), "launch", "lights.launch.py"] ) ), launch_arguments={"namespace": namespace}.items(), @@ -89,7 +83,7 @@ def generate_launch_description(): battery_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_battery"), "launch", "battery.launch.py"] + [FindPackageShare("husarion_ugv_battery"), "launch", "battery.launch.py"] ), ), launch_arguments={"namespace": namespace}.items(), @@ -98,17 +92,16 @@ def generate_launch_description(): ekf_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_localization"), "launch", "localization.launch.py"] + [FindPackageShare("husarion_ugv_localization"), "launch", "localization.launch.py"] ) ), launch_arguments={"namespace": namespace}.items(), - condition=IfCondition(use_ekf), ) manager_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_manager"), "launch", "manager.launch.py"] + [FindPackageShare("husarion_ugv_manager"), "launch", "manager.launch.py"] ) ), condition=UnlessCondition(disable_manager), @@ -128,8 +121,7 @@ def generate_launch_description(): actions = [ declare_disable_manager_arg, declare_namespace_arg, - declare_use_ekf_arg, - welcome_msg, + welcome_info, controller_launch, system_monitor_launch, delayed_action, diff --git a/panther_bringup/package.xml b/husarion_ugv_bringup/package.xml similarity index 81% rename from panther_bringup/package.xml rename to husarion_ugv_bringup/package.xml index ebd80a4e..9bc144d1 100644 --- a/panther_bringup/package.xml +++ b/husarion_ugv_bringup/package.xml @@ -1,9 +1,9 @@ - panther_bringup + husarion_ugv_bringup 2.1.2 - Default launch files and configuration used to start Husarion Panther robot + Default launch files and configuration used to start Husarion UGV Husarion Apache License 2.0 @@ -15,15 +15,15 @@ ament_cmake + husarion_ugv_battery + husarion_ugv_controller + husarion_ugv_diagnostics + husarion_ugv_lights + husarion_ugv_localization + husarion_ugv_manager + husarion_ugv_utils launch launch_ros - panther_battery - panther_controller - panther_diagnostics - panther_lights - panther_localization - panther_manager - panther_utils ament_cmake diff --git a/panther_controller/CHANGELOG.rst b/husarion_ugv_controller/CHANGELOG.rst similarity index 100% rename from panther_controller/CHANGELOG.rst rename to husarion_ugv_controller/CHANGELOG.rst diff --git a/panther_localization/CMakeLists.txt b/husarion_ugv_controller/CMakeLists.txt similarity index 82% rename from panther_localization/CMakeLists.txt rename to husarion_ugv_controller/CMakeLists.txt index 33da28a6..9cf8423a 100644 --- a/panther_localization/CMakeLists.txt +++ b/husarion_ugv_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(panther_localization) +project(husarion_ugv_controller) find_package(ament_cmake REQUIRED) diff --git a/husarion_ugv_controller/CONFIGURATION.md b/husarion_ugv_controller/CONFIGURATION.md new file mode 100644 index 00000000..01a40d15 --- /dev/null +++ b/husarion_ugv_controller/CONFIGURATION.md @@ -0,0 +1,9 @@ +# husarion_ugv_controller + +## Changing Velocity Smoothing Parameters + +The default drive controller is based on [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 [husarion_ugv_controller](https://github.com/husarion/panther_ros/tree/ros2-devel/husarion_ugv_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/husarion_ugv_controller/README.md similarity index 96% rename from panther_controller/README.md rename to husarion_ugv_controller/README.md index ac600dc7..509a5e4e 100644 --- a/panther_controller/README.md +++ b/husarion_ugv_controller/README.md @@ -1,6 +1,6 @@ -# panther_controller +# husarion_ugv_controller -The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot. +The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion UGV. ## Launch Files diff --git a/panther_controller/config/WH01_controller.yaml b/husarion_ugv_controller/config/WH01_controller.yaml similarity index 99% rename from panther_controller/config/WH01_controller.yaml rename to husarion_ugv_controller/config/WH01_controller.yaml index 4187aa50..da21d73e 100644 --- a/panther_controller/config/WH01_controller.yaml +++ b/husarion_ugv_controller/config/WH01_controller.yaml @@ -55,7 +55,7 @@ enable_odom_tf: false - cmd_vel_timeout: 0.5 + cmd_vel_timeout: 0.2 #publish_limited_velocity: true use_stamped_vel: false diff --git a/panther_controller/config/WH02_controller.yaml b/husarion_ugv_controller/config/WH02_controller.yaml similarity index 99% rename from panther_controller/config/WH02_controller.yaml rename to husarion_ugv_controller/config/WH02_controller.yaml index 6d109509..0e6e8ee5 100644 --- a/panther_controller/config/WH02_controller.yaml +++ b/husarion_ugv_controller/config/WH02_controller.yaml @@ -56,7 +56,7 @@ enable_odom_tf: false - cmd_vel_timeout: 0.5 + cmd_vel_timeout: 0.2 #publish_limited_velocity: true use_stamped_vel: false diff --git a/panther_controller/config/WH04_controller.yaml b/husarion_ugv_controller/config/WH04_controller.yaml similarity index 98% rename from panther_controller/config/WH04_controller.yaml rename to husarion_ugv_controller/config/WH04_controller.yaml index 25987350..2ae5cb34 100644 --- a/panther_controller/config/WH04_controller.yaml +++ b/husarion_ugv_controller/config/WH04_controller.yaml @@ -32,7 +32,7 @@ wheel_separation: 0.616 wheel_radius: 0.1016 - # todo: check it for panther + # TODO: check it # For skid drive kinematics it will act as ICR coefficient, kinematic model with ICR # coefficient isn't totally accurate and this coefficient can differ for various ground types wheel_separation_multiplier: 1.0 @@ -56,7 +56,7 @@ enable_odom_tf: false - cmd_vel_timeout: 0.5 + cmd_vel_timeout: 0.2 #publish_limited_velocity: true use_stamped_vel: false diff --git a/husarion_ugv_controller/config/WH05_controller.yaml b/husarion_ugv_controller/config/WH05_controller.yaml new file mode 100644 index 00000000..e9cfd598 --- /dev/null +++ b/husarion_ugv_controller/config/WH05_controller.yaml @@ -0,0 +1,84 @@ +/**: + controller_manager: + ros__parameters: + update_rate: 100 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + drive_controller: + type: diff_drive_controller/DiffDriveController + imu_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + + # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications + imu_broadcaster: + ros__parameters: + use_namespace_as_sensor_name_prefix: true + + sensor_name: imu + frame_id: imu_link + # orientation_stddev: 4.3e-2 rad determined experimentally + static_covariance_orientation: [1.8e-3, 0.0, 0.0, 0.0, 1.8e-3, 0.0, 0.0, 0.0, 1.8e-3] + # angular_velocity_stdev: 0.59 deg/s (0.01 rad/s) gyroscope white noise sigma, according to the manual + static_covariance_angular_velocity: [1.0e-4, 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, 0.0, 1.0e-4] + # linear_acceleration_stdev: 2.8 mg (0.0275 m/s^2) accelerometer white noise sigma, according to the manual + static_covariance_linear_acceleration: [7.6e-4, 0.0, 0.0, 0.0, 7.6e-4, 0.0, 0.0, 0.0, 7.6e-4] + + drive_controller: + ros__parameters: + left_wheel_names: [fl_wheel_joint, rl_wheel_joint] + right_wheel_names: [fr_wheel_joint, rr_wheel_joint] + + wheel_separation: 0.45 + wheel_radius: 0.1348 + + # For skid drive kinematics it will act as ICR coefficient, kinematic model with ICR + # coefficient isn't totally accurate and this coefficient can differ for various ground types + wheel_separation_multiplier: 1.5 + + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 100.0 + odom_frame_id: odom + base_frame_id: base_link + twist_covariance_diagonal: [5.4e-5, 5.4e-5, 0.0, 0.0, 0.0, 1.9e-4] # Values measured experimentally + + # Whether to use feedback or commands for odometry calculations + open_loop: false + + # Update odometry from velocity + # in sensor fusion only velocity is used and with this setting it is more accurate + position_feedback: false + # velocity computation filtering + velocity_rolling_window_size: 1 + + enable_odom_tf: false + + cmd_vel_timeout: 0.2 + #publish_limited_velocity: true + use_stamped_vel: false + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + max_velocity: 1.5 # m/s + # min_velocity - When unspecified `min_velocity=-max_velocity` + max_acceleration: 2.1 # m/s^2 + # min_acceleration - When unspecified, `min_acceleration=-max_acceleration` + max_jerk: 0.0 # m/s^3 + + angular: + z: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + max_velocity: 4.0 # rad/s + # min_velocity - When unspecified `min_velocity=-max_velocity` + max_acceleration: 5.74 # rad/s^2 + # min_acceleration - When unspecified, `min_acceleration=-max_acceleration` + max_jerk: 0.0 # rad/s^3 diff --git a/panther_controller/launch/controller.launch.py b/husarion_ugv_controller/launch/controller.launch.py similarity index 79% rename from panther_controller/launch/controller.launch.py rename to husarion_ugv_controller/launch/controller.launch.py index 0ad64e58..843bbbc6 100644 --- a/panther_controller/launch/controller.launch.py +++ b/husarion_ugv_controller/launch/controller.launch.py @@ -34,6 +34,21 @@ def generate_launch_description(): + husarion_ugv_controller_common_dir = PathJoinSubstitution( + ["/config", "husarion_ugv_controller"] + ) + + robot_model = LaunchConfiguration("robot_model") + robot_description_pkg = PythonExpression(["'", robot_model, "_description'"]) + robot_description_common_dir = PathJoinSubstitution(["/config", robot_description_pkg]) + + declare_robot_model_arg = DeclareLaunchArgument( + "robot_model", + default_value=EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther"), + description="Specify robot model", + choices=["lynx", "panther"], + ) + battery_config_path = LaunchConfiguration("battery_config_path") declare_battery_config_path_arg = DeclareLaunchArgument( "battery_config_path", @@ -48,32 +63,30 @@ def generate_launch_description(): declare_components_config_path_arg = DeclareLaunchArgument( "components_config_path", default_value=PathJoinSubstitution( - [FindPackageShare("panther_description"), "config", "components.yaml"] + [robot_description_common_dir, "config", "components.yaml"] ), description=( "Additional components configuration file. Components described in this file " - "are dynamically included in Panther's urdf." - "Panther options are described here " + "are dynamically included in robot's URDF." + "Available options are described in the manual: " "https://husarion.com/manuals/panther/panther-options/" ), ) - wheel_type = LaunchConfiguration( - "wheel_type" - ) # wheel_type must be before controller_config_path + wheel_type = LaunchConfiguration("wheel_type") controller_config_path = LaunchConfiguration("controller_config_path") declare_controller_config_path_arg = DeclareLaunchArgument( "controller_config_path", default_value=PathJoinSubstitution( [ - FindPackageShare("panther_controller"), + husarion_ugv_controller_common_dir, "config", PythonExpression(["'", wheel_type, "_controller.yaml'"]), ] ), description=( "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify" + " 'husarion_ugv_controller/config/{wheel_type}_controller.yaml'. You can also specify" " the path to your custom controller configuration file here. " ), ) @@ -96,7 +109,7 @@ def generate_launch_description(): choices=["True", "true", "False", "false"], ) - wheel_config_path = LaunchConfiguration("wheel_config_path") + use_sim = LaunchConfiguration("use_sim") declare_use_sim_arg = DeclareLaunchArgument( "use_sim", default_value="False", @@ -104,11 +117,12 @@ def generate_launch_description(): choices=["True", "true", "False", "false"], ) + wheel_config_path = LaunchConfiguration("wheel_config_path") declare_wheel_config_path_arg = DeclareLaunchArgument( "wheel_config_path", default_value=PathJoinSubstitution( [ - FindPackageShare("panther_description"), + FindPackageShare(robot_description_pkg), "config", PythonExpression(["'", wheel_type, ".yaml'"]), ] @@ -120,33 +134,37 @@ def generate_launch_description(): ), ) - use_sim = LaunchConfiguration("use_sim") + default_wheel_type = {"lynx": "WH05", "panther": "WH01"} declare_wheel_type_arg = DeclareLaunchArgument( "wheel_type", - default_value="WH01", + default_value=PythonExpression([f"{default_wheel_type}['", robot_model, "']"]), description=( - "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." + "Specify the wheel type. If the selected wheel type is not 'custom', " + "the 'wheel_config_path' and 'controller_config_path' arguments will be " + "automatically adjusted and can be omitted." ), - choices=["WH01", "WH02", "WH04", "custom"], + choices=["WH01", "WH02", "WH04", "WH05", "custom"], ) # Get URDF via xacro + robot_description_file = PythonExpression(["'", robot_model, ".urdf.xacro'"]) + imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168") + imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028") + imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083") + imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14") + imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57") + imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0") robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [ - FindPackageShare("panther_description"), + FindPackageShare(robot_description_pkg), "urdf", - "panther.urdf.xacro", + robot_description_file, ] ), - " panther_version:=", - os.environ.get("PANTHER_ROBOT_VERSION", "1.0"), " use_sim:=", use_sim, " wheel_config_file:=", @@ -156,9 +174,9 @@ def generate_launch_description(): " battery_config_file:=", battery_config_path, " imu_xyz:=", - f"\"{os.environ.get('PANTHER_IMU_LOCALIZATION_X', '0.168')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Y', '0.028')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Z', '0.083')}\"", + f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'", " imu_rpy:=", - f"\"{os.environ.get('PANTHER_IMU_ORIENTATION_R', '3.14')} {os.environ.get('PANTHER_IMU_ORIENTATION_P', '-1.57')} {os.environ.get('PANTHER_IMU_ORIENTATION_Y', '0.0')}\"", + f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'", " namespace:=", namespace, " components_config_path:=", @@ -186,7 +204,7 @@ def generate_launch_description(): ("hardware_controller/fan_enable", "hardware/fan_enable"), ("hardware_controller/io_state", "hardware/io_state"), ("hardware_controller/led_control_enable", "hardware/led_control_enable"), - ("hardware_controller/motor_controllers_state", "hardware/motor_controllers_state"), + ("hardware_controller/robot_driver_state", "hardware/robot_driver_state"), ("hardware_controller/motor_power_enable", "hardware/motor_power_enable"), ("imu_broadcaster/imu", "imu/data"), ("imu_broadcaster/transition_event", "_imu_broadcaster/transition_event"), @@ -271,7 +289,8 @@ def generate_launch_description(): actions = [ declare_battery_config_path_arg, - declare_wheel_type_arg, # wheel_type must be before controller_config_path + declare_robot_model_arg, # robot_model is used by wheel_type + declare_wheel_type_arg, # wheel_type is used by controller_config_path declare_components_config_path_arg, declare_controller_config_path_arg, declare_namespace_arg, diff --git a/panther_controller/package.xml b/husarion_ugv_controller/package.xml similarity index 86% rename from panther_controller/package.xml rename to husarion_ugv_controller/package.xml index 5a72a325..591bd912 100644 --- a/panther_controller/package.xml +++ b/husarion_ugv_controller/package.xml @@ -1,9 +1,9 @@ - panther_controller + husarion_ugv_controller 2.1.2 - ros2 controllers configuration for Panther + ros2 controllers configuration for Husarion UGV Husarion Apache License 2.0 @@ -18,13 +18,14 @@ controller_manager diff_drive_controller + husarion_ugv_hardware_interfaces imu_sensor_broadcaster joint_state_broadcaster launch launch_ros + lynx_description mecanum_drive_controller panther_description - panther_hardware_interfaces robot_state_publisher xacro diff --git a/panther_diagnostics/CHANGELOG.rst b/husarion_ugv_diagnostics/CHANGELOG.rst similarity index 100% rename from panther_diagnostics/CHANGELOG.rst rename to husarion_ugv_diagnostics/CHANGELOG.rst diff --git a/panther_diagnostics/CMakeLists.txt b/husarion_ugv_diagnostics/CMakeLists.txt similarity index 97% rename from panther_diagnostics/CMakeLists.txt rename to husarion_ugv_diagnostics/CMakeLists.txt index 8794efbe..64b9751d 100644 --- a/panther_diagnostics/CMakeLists.txt +++ b/husarion_ugv_diagnostics/CMakeLists.txt @@ -8,7 +8,7 @@ if(USE_SUPERBUILD) include(cmake/SuperBuild.cmake) return() else() - project(panther_diagnostics) + project(husarion_ugv_diagnostics) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -21,7 +21,7 @@ set(PACKAGE_DEPENDENCIES diagnostic_updater generate_parameter_library panther_msgs - panther_utils + husarion_ugv_utils PkgConfig rclcpp std_msgs) diff --git a/panther_diagnostics/README.md b/husarion_ugv_diagnostics/README.md similarity index 95% rename from panther_diagnostics/README.md rename to husarion_ugv_diagnostics/README.md index d3e213bb..02d37021 100644 --- a/panther_diagnostics/README.md +++ b/husarion_ugv_diagnostics/README.md @@ -1,6 +1,6 @@ -# panther_diagnostics +# husarion_ugv_diagnostics -Package containing nodes monitoring and publishing the Built-in Computer status of Husarion Panther robot. +Package containing nodes monitoring and publishing the Built-in Computer status of Husarion UGV. ## Launch Files diff --git a/panther_diagnostics/cmake/SuperBuild.cmake b/husarion_ugv_diagnostics/cmake/SuperBuild.cmake similarity index 97% rename from panther_diagnostics/cmake/SuperBuild.cmake rename to husarion_ugv_diagnostics/cmake/SuperBuild.cmake index 4370b4cd..83fdadbb 100644 --- a/panther_diagnostics/cmake/SuperBuild.cmake +++ b/husarion_ugv_diagnostics/cmake/SuperBuild.cmake @@ -30,7 +30,7 @@ ExternalProject_Add( BUILD_IN_SOURCE 1) ExternalProject_Add( - ep_panther_diagnostics + ep_husarion_ugv_diagnostics DEPENDS ${DEPENDENCIES} SOURCE_DIR ${PROJECT_SOURCE_DIR} CMAKE_ARGS -DUSE_SUPERBUILD=OFF diff --git a/panther_diagnostics/config/system_monitor.yaml b/husarion_ugv_diagnostics/config/system_monitor.yaml similarity index 100% rename from panther_diagnostics/config/system_monitor.yaml rename to husarion_ugv_diagnostics/config/system_monitor.yaml diff --git a/panther_diagnostics/include/panther_diagnostics/filesystem.hpp b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/filesystem.hpp similarity index 93% rename from panther_diagnostics/include/panther_diagnostics/filesystem.hpp rename to husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/filesystem.hpp index fa2291d6..17825312 100644 --- a/panther_diagnostics/include/panther_diagnostics/filesystem.hpp +++ b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/filesystem.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_ -#define PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_ +#ifndef HUSARION_UGV_DIAGNOSTICS_FILESYSTEM_HPP_ +#define HUSARION_UGV_DIAGNOSTICS_FILESYSTEM_HPP_ #include #include -namespace panther_diagnostics +namespace husarion_ugv_diagnostics { /** @@ -110,6 +110,6 @@ class Filesystem : public FilesystemInterface } }; -} // namespace panther_diagnostics +} // namespace husarion_ugv_diagnostics -#endif // PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_ +#endif // HUSARION_UGV_DIAGNOSTICS_FILESYSTEM_HPP_ diff --git a/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/system_monitor_node.hpp similarity index 87% rename from panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp rename to husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/system_monitor_node.hpp index c840a246..0532822f 100644 --- a/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp +++ b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/system_monitor_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ -#define PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ +#ifndef HUSARION_UGV_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ +#define HUSARION_UGV_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ #include @@ -24,12 +24,12 @@ #include "system_monitor_parameters.hpp" -#include "panther_diagnostics/filesystem.hpp" -#include "panther_diagnostics/types.hpp" +#include "husarion_ugv_diagnostics/filesystem.hpp" +#include "husarion_ugv_diagnostics/types.hpp" using namespace std::chrono_literals; -namespace panther_diagnostics +namespace husarion_ugv_diagnostics { class SystemMonitorNode : public rclcpp::Node @@ -81,5 +81,5 @@ class SystemMonitorNode : public rclcpp::Node static constexpr char kTemperatureInfoFilename[] = "/sys/class/thermal/thermal_zone0/temp"; static constexpr char kRootDirectory[] = "/"; }; -} // namespace panther_diagnostics -#endif // PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ +} // namespace husarion_ugv_diagnostics +#endif // HUSARION_UGV_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ diff --git a/panther_diagnostics/include/panther_diagnostics/types.hpp b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/types.hpp similarity index 82% rename from panther_diagnostics/include/panther_diagnostics/types.hpp rename to husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/types.hpp index 1ed2c88c..4108b56d 100644 --- a/panther_diagnostics/include/panther_diagnostics/types.hpp +++ b/husarion_ugv_diagnostics/include/husarion_ugv_diagnostics/types.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_DIAGNOSTICS_TYPES_HPP_ -#define PANTHER_DIAGNOSTICS_TYPES_HPP_ +#ifndef HUSARION_UGV_DIAGNOSTICS_TYPES_HPP_ +#define HUSARION_UGV_DIAGNOSTICS_TYPES_HPP_ #include -namespace panther_diagnostics +namespace husarion_ugv_diagnostics { /** @@ -35,6 +35,6 @@ struct SystemStatus float disk_usage; }; -} // namespace panther_diagnostics +} // namespace husarion_ugv_diagnostics -#endif // PANTHER_DIAGNOSTICS_TYPES_HPP_ +#endif // HUSARION_UGV_DIAGNOSTICS_TYPES_HPP_ diff --git a/panther_diagnostics/launch/system_monitor.launch.py b/husarion_ugv_diagnostics/launch/system_monitor.launch.py similarity index 97% rename from panther_diagnostics/launch/system_monitor.launch.py rename to husarion_ugv_diagnostics/launch/system_monitor.launch.py index e2d2ab70..34a82815 100644 --- a/panther_diagnostics/launch/system_monitor.launch.py +++ b/husarion_ugv_diagnostics/launch/system_monitor.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): ) system_monitor_node = Node( - package="panther_diagnostics", + package="husarion_ugv_diagnostics", executable="system_monitor_node", name="system_monitor", namespace=namespace, diff --git a/panther_diagnostics/package.xml b/husarion_ugv_diagnostics/package.xml similarity index 87% rename from panther_diagnostics/package.xml rename to husarion_ugv_diagnostics/package.xml index f51b357e..c7980707 100644 --- a/panther_diagnostics/package.xml +++ b/husarion_ugv_diagnostics/package.xml @@ -1,9 +1,9 @@ - panther_diagnostics + husarion_ugv_diagnostics 2.1.2 - Package for diagnosting usage of OS on the Panther Robot + Package for diagnosting usage of OS on the Husarion UGV robot Husarion Apache License 2.0 @@ -20,8 +20,8 @@ diagnostic_msgs diagnostic_updater generate_parameter_library + husarion_ugv_utils panther_msgs - panther_utils rclcpp std_msgs diff --git a/panther_diagnostics/src/main.cpp b/husarion_ugv_diagnostics/src/main.cpp similarity index 80% rename from panther_diagnostics/src/main.cpp rename to husarion_ugv_diagnostics/src/main.cpp index 8d09517f..61cb38cc 100644 --- a/panther_diagnostics/src/main.cpp +++ b/husarion_ugv_diagnostics/src/main.cpp @@ -18,15 +18,15 @@ #include -#include "panther_diagnostics/filesystem.hpp" -#include "panther_diagnostics/system_monitor_node.hpp" +#include "husarion_ugv_diagnostics/filesystem.hpp" +#include "husarion_ugv_diagnostics/system_monitor_node.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto filesystem = std::make_shared(); - auto system_monitor_node = std::make_shared( + auto filesystem = std::make_shared(); + auto system_monitor_node = std::make_shared( "system_monitor", filesystem); try { diff --git a/panther_diagnostics/src/system_monitor_node.cpp b/husarion_ugv_diagnostics/src/system_monitor_node.cpp similarity index 89% rename from panther_diagnostics/src/system_monitor_node.cpp rename to husarion_ugv_diagnostics/src/system_monitor_node.cpp index bc921a75..f25bcbe3 100644 --- a/panther_diagnostics/src/system_monitor_node.cpp +++ b/husarion_ugv_diagnostics/src/system_monitor_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_diagnostics/system_monitor_node.hpp" +#include "husarion_ugv_diagnostics/system_monitor_node.hpp" #include #include @@ -24,13 +24,13 @@ #include "panther_msgs/msg/system_status.hpp" -#include "panther_utils/common_utilities.hpp" -#include "panther_utils/ros_utils.hpp" +#include "husarion_ugv_utils/common_utilities.hpp" +#include "husarion_ugv_utils/ros_utils.hpp" -#include "panther_diagnostics/filesystem.hpp" -#include "panther_diagnostics/types.hpp" +#include "husarion_ugv_diagnostics/filesystem.hpp" +#include "husarion_ugv_diagnostics/types.hpp" -namespace panther_diagnostics +namespace husarion_ugv_diagnostics { SystemMonitorNode::SystemMonitorNode( @@ -101,7 +101,7 @@ float SystemMonitorNode::GetCPUMeanUsage(const std::vector & usages) cons }); auto sum = std::accumulate(usages.begin(), usages.end(), 0.0); - auto mean_usage = panther_utils::common_utilities::SetPrecision(sum / usages.size(), 2); + auto mean_usage = husarion_ugv_utils::common_utilities::SetPrecision(sum / usages.size(), 2); return mean_usage; } @@ -112,7 +112,7 @@ float SystemMonitorNode::GetCPUTemperature() const try { const auto temperature_str = filesystem_->ReadFile(kTemperatureInfoFilename); - temperature = panther_utils::common_utilities::SetPrecision( + temperature = husarion_ugv_utils::common_utilities::SetPrecision( std::stof(temperature_str) / 1000.0, 2); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM( @@ -127,7 +127,8 @@ 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); + const auto ram_usage = husarion_ugv_utils::common_utilities::CountPercentage( + total - available, total); return ram_usage; } @@ -138,7 +139,8 @@ float SystemMonitorNode::GetDiskUsage() const try { const auto capacity = filesystem_->GetSpaceCapacity(kRootDirectory); const auto available = filesystem_->GetSpaceAvailable(kRootDirectory); - disk_usage = panther_utils::common_utilities::CountPercentage(capacity - available, capacity); + disk_usage = husarion_ugv_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()); @@ -196,4 +198,4 @@ void SystemMonitorNode::DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapp status.summary(error_level, message); } -} // namespace panther_diagnostics +} // namespace husarion_ugv_diagnostics diff --git a/panther_diagnostics/test/integration/system_monitor_node.test.py b/husarion_ugv_diagnostics/test/integration/system_monitor_node.test.py similarity index 97% rename from panther_diagnostics/test/integration/system_monitor_node.test.py rename to husarion_ugv_diagnostics/test/integration/system_monitor_node.test.py index dc895713..53b34bd4 100644 --- a/panther_diagnostics/test/integration/system_monitor_node.test.py +++ b/husarion_ugv_diagnostics/test/integration/system_monitor_node.test.py @@ -30,7 +30,7 @@ def generate_test_description(): system_monitor_node = Node( - package="panther_diagnostics", + package="husarion_ugv_diagnostics", executable="system_monitor_node", ) diff --git a/panther_diagnostics/test/unit/test_filesystem.cpp b/husarion_ugv_diagnostics/test/unit/test_filesystem.cpp similarity index 91% rename from panther_diagnostics/test/unit/test_filesystem.cpp rename to husarion_ugv_diagnostics/test/unit/test_filesystem.cpp index 3e9c8755..24f7a654 100644 --- a/panther_diagnostics/test/unit/test_filesystem.cpp +++ b/husarion_ugv_diagnostics/test/unit/test_filesystem.cpp @@ -18,7 +18,7 @@ #include #include -#include "panther_diagnostics/filesystem.hpp" +#include "husarion_ugv_diagnostics/filesystem.hpp" class TestFilesystem : public testing::Test { @@ -29,12 +29,13 @@ class TestFilesystem : public testing::Test void RemoveTestFile(const std::string & file_path); protected: - std::shared_ptr filesystem_; + std::shared_ptr filesystem_; static constexpr char kDummyString[] = "Hello World!"; }; -TestFilesystem::TestFilesystem() : filesystem_(std::make_shared()) +TestFilesystem::TestFilesystem() +: filesystem_(std::make_shared()) { } diff --git a/panther_diagnostics/test/unit/test_system_monitor_node.cpp b/husarion_ugv_diagnostics/test/unit/test_system_monitor_node.cpp similarity index 83% rename from panther_diagnostics/test/unit/test_system_monitor_node.cpp rename to husarion_ugv_diagnostics/test/unit/test_system_monitor_node.cpp index 8c93ff24..dd6ad053 100644 --- a/panther_diagnostics/test/unit/test_system_monitor_node.cpp +++ b/husarion_ugv_diagnostics/test/unit/test_system_monitor_node.cpp @@ -20,10 +20,10 @@ #include #include -#include "panther_diagnostics/filesystem.hpp" -#include "panther_diagnostics/system_monitor_node.hpp" +#include "husarion_ugv_diagnostics/filesystem.hpp" +#include "husarion_ugv_diagnostics/system_monitor_node.hpp" -class MockFilesystem : public panther_diagnostics::FilesystemInterface +class MockFilesystem : public husarion_ugv_diagnostics::FilesystemInterface { public: MOCK_METHOD( @@ -35,38 +35,38 @@ class MockFilesystem : public panther_diagnostics::FilesystemInterface MOCK_METHOD(std::string, ReadFile, (const std::string & file_path), (const, override)); }; -class SystemMonitorNodeWrapper : public panther_diagnostics::SystemMonitorNode +class SystemMonitorNodeWrapper : public husarion_ugv_diagnostics::SystemMonitorNode { public: SystemMonitorNodeWrapper(std::shared_ptr filesystem) - : panther_diagnostics::SystemMonitorNode("test_system_statics", filesystem) + : husarion_ugv_diagnostics::SystemMonitorNode("test_system_statics", filesystem) { } std::vector GetCoresUsages() const { - return panther_diagnostics::SystemMonitorNode::GetCoresUsages(); + return husarion_ugv_diagnostics::SystemMonitorNode::GetCoresUsages(); } float GetCPUMeanUsage(const std::vector & usages) const { - return panther_diagnostics::SystemMonitorNode::GetCPUMeanUsage(usages); + return husarion_ugv_diagnostics::SystemMonitorNode::GetCPUMeanUsage(usages); } float GetCPUTemperature() const { - return panther_diagnostics::SystemMonitorNode::GetCPUTemperature(); + return husarion_ugv_diagnostics::SystemMonitorNode::GetCPUTemperature(); } - float GetRAMUsage() const { return panther_diagnostics::SystemMonitorNode::GetRAMUsage(); } + float GetRAMUsage() const { return husarion_ugv_diagnostics::SystemMonitorNode::GetRAMUsage(); } - float GetDiskUsage() const { return panther_diagnostics::SystemMonitorNode::GetDiskUsage(); } + float GetDiskUsage() const { return husarion_ugv_diagnostics::SystemMonitorNode::GetDiskUsage(); } panther_msgs::msg::SystemStatus SystemStatusToMessage( - const panther_diagnostics::SystemStatus & status) + const husarion_ugv_diagnostics::SystemStatus & status) { - return panther_diagnostics::SystemMonitorNode::SystemStatusToMessage(status); + return husarion_ugv_diagnostics::SystemMonitorNode::SystemStatusToMessage(status); } }; @@ -160,7 +160,7 @@ TEST_F(TestSystemMonitorNode, GetDiskUsageInvalidFilesystem) TEST_F(TestSystemMonitorNode, SystemStatusToMessage) { - panther_diagnostics::SystemStatus test_status; + husarion_ugv_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; diff --git a/panther_gazebo/CHANGELOG.rst b/husarion_ugv_gazebo/CHANGELOG.rst similarity index 100% rename from panther_gazebo/CHANGELOG.rst rename to husarion_ugv_gazebo/CHANGELOG.rst diff --git a/panther_gazebo/CMakeLists.txt b/husarion_ugv_gazebo/CMakeLists.txt similarity index 79% rename from panther_gazebo/CMakeLists.txt rename to husarion_ugv_gazebo/CMakeLists.txt index cae0aaa3..962f6db5 100644 --- a/panther_gazebo/CMakeLists.txt +++ b/husarion_ugv_gazebo/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(panther_gazebo) +project(husarion_ugv_gazebo) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -28,21 +28,21 @@ find_package(Qt5 REQUIRED COMPONENTS Core Quick QuickControls2) 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) +add_library(estop_system SHARED src/estop_system.cpp) ament_target_dependencies( - panther_simulation_plugins + estop_system hardware_interface ign_ros2_control rclcpp rclcpp_lifecycle std_msgs std_srvs) -target_link_libraries(panther_simulation_plugins ignition-gazebo6) +target_link_libraries(estop_system 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 +add_library(EStop SHARED include/husarion_ugv_gazebo/gui/e_stop.hpp src/gui/e_stop.cpp ${resources_rcc}) ament_target_dependencies(EStop rclcpp std_srvs) target_link_libraries( @@ -60,7 +60,7 @@ target_link_libraries(LEDStrip ignition-gazebo6 ignition-msgs8 ignition-plugin1 ignition-transport11) install( - TARGETS panther_simulation_plugins + TARGETS estop_system RUNTIME DESTINATION bin LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) @@ -86,10 +86,8 @@ endif() ament_export_include_directories(include) -pluginlib_export_plugin_description_file(ign_ros2_control - panther_simulation_plugins.xml) +pluginlib_export_plugin_description_file(ign_ros2_control plugins.xml) -ament_environment_hooks( - "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in") +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/setup_envs.sh.in") ament_package() diff --git a/panther_gazebo/CONFIGURATION.md b/husarion_ugv_gazebo/CONFIGURATION.md similarity index 71% rename from panther_gazebo/CONFIGURATION.md rename to husarion_ugv_gazebo/CONFIGURATION.md index c41cc924..3ad174b3 100644 --- a/panther_gazebo/CONFIGURATION.md +++ b/husarion_ugv_gazebo/CONFIGURATION.md @@ -1,8 +1,8 @@ -# panther_gazebo +# husarion_ugv_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 system is utilized to publish the 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. @@ -33,12 +33,12 @@ To obtain GPS data in Ignition, follow these steps: ## 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. +It is possible to simulate the battery operation of the simulated robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin.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). +- `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 robot power consumption, please refer to [Battery & Charging Documentation](https://husarion.com/manuals/panther/#battery--charging). > [!NOTE] > @@ -51,11 +51,11 @@ Unfortunately, there is no straightforward way to exchange `LinearBatteryPlugin` 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 +ign service --service /model/{robot_model}/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 +ign service --service /model/{robot_model}/battery/battery/recharge/stop --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 ``` diff --git a/panther_gazebo/README.md b/husarion_ugv_gazebo/README.md similarity index 95% rename from panther_gazebo/README.md rename to husarion_ugv_gazebo/README.md index a4d3bb85..3b573404 100644 --- a/panther_gazebo/README.md +++ b/husarion_ugv_gazebo/README.md @@ -1,4 +1,4 @@ -# panther_gazebo +# husarion_ugv_gazebo 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. @@ -11,7 +11,7 @@ The package contains a launch file and source files used to run the robot simula ## Configuration Files -- [`battery_plugin_config.yaml`](./config/battery_plugin_config.yaml): Simulated LinearBatteryPlugin configuration. +- [`battery_plugin.yaml`](./config/battery_plugin.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. @@ -26,7 +26,7 @@ The package contains a launch file and source files used to run the robot simula - `hardware/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop. - `hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop. -### GzPantherSystem +### EStopSystem 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. diff --git a/panther_gazebo/config/battery_plugin_config.yaml b/husarion_ugv_gazebo/config/battery_plugin.yaml similarity index 82% rename from panther_gazebo/config/battery_plugin_config.yaml rename to husarion_ugv_gazebo/config/battery_plugin.yaml index 45eaac5e..2564ebc3 100644 --- a/panther_gazebo/config/battery_plugin_config.yaml +++ b/husarion_ugv_gazebo/config/battery_plugin.yaml @@ -1,6 +1,6 @@ # Parameters for the Gazebo LinearBatteryPlugin -# For more information on Panther power consumption, please refer to: -# https://husarion.com/manuals/panther/#battery--charging +# For more information on robots power consumption, please refer to: +# Panther: https://husarion.com/manuals/panther/#battery--charging # If set to true, the battery will discharge and if it depletes completely, the robot will stop moving. simulate_discharging: false diff --git a/panther_gazebo/config/gz_bridge.yaml b/husarion_ugv_gazebo/config/gz_bridge.yaml similarity index 100% rename from panther_gazebo/config/gz_bridge.yaml rename to husarion_ugv_gazebo/config/gz_bridge.yaml diff --git a/panther_gazebo/config/teleop_with_estop.config b/husarion_ugv_gazebo/config/teleop_with_estop.config similarity index 100% rename from panther_gazebo/config/teleop_with_estop.config rename to husarion_ugv_gazebo/config/teleop_with_estop.config diff --git a/panther_gazebo/hooks/panther_gazebo.sh.in b/husarion_ugv_gazebo/hooks/setup_envs.sh.in similarity index 100% rename from panther_gazebo/hooks/panther_gazebo.sh.in rename to husarion_ugv_gazebo/hooks/setup_envs.sh.in diff --git a/panther_gazebo/include/panther_gazebo/gz_panther_system.hpp b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/estop_system.hpp similarity index 87% rename from panther_gazebo/include/panther_gazebo/gz_panther_system.hpp rename to husarion_ugv_gazebo/include/husarion_ugv_gazebo/estop_system.hpp index 6978fa71..6d8883c7 100644 --- a/panther_gazebo/include/panther_gazebo/gz_panther_system.hpp +++ b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/estop_system.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_GAZEBO_GZ_PANTHER_SYSTEM -#define PANTHER_GAZEBO_GZ_PANTHER_SYSTEM +#ifndef HUSARION_UGV_GAZEBO_ESTOP_SYSTEM +#define HUSARION_UGV_GAZEBO_ESTOP_SYSTEM #include @@ -27,18 +27,18 @@ #include #include -namespace panther_gazebo +namespace husarion_ugv_gazebo { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using BoolMsg = std_msgs::msg::Bool; using TriggerSrv = std_srvs::srv::Trigger; /** - * @brief Main class for the Panther System which implements a simulated `ros2_control` + * @brief Main class for the Husarion UGV which implements a simulated `ros2_control` * `hardware_interface::SystemInterface`. This class inherits `ign_ros2_control::IgnitionSystem` * and implements additional functionalities like E-stop handling. */ -class GzPantherSystem : public ign_ros2_control::IgnitionSystem +class EStopSystem : public ign_ros2_control::IgnitionSystem { public: CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info) override; @@ -69,6 +69,6 @@ class GzPantherSystem : public ign_ros2_control::IgnitionSystem rclcpp::Service::SharedPtr e_stop_trigger_service_; }; -} // namespace panther_gazebo +} // namespace husarion_ugv_gazebo -#endif // PANTHER_GAZEBO_GZ_PANTHER_SYSTEM +#endif // HUSARION_UGV_GAZEBO_ESTOP_SYSTEM diff --git a/panther_gazebo/include/panther_gazebo/gui/e_stop.hpp b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/gui/e_stop.hpp similarity index 89% rename from panther_gazebo/include/panther_gazebo/gui/e_stop.hpp rename to husarion_ugv_gazebo/include/husarion_ugv_gazebo/gui/e_stop.hpp index 055db7e1..962f5b20 100644 --- a/panther_gazebo/include/panther_gazebo/gui/e_stop.hpp +++ b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/gui/e_stop.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_GAZEBO_GUI_E_STOP_HPP_ -#define PANTHER_GAZEBO_GUI_E_STOP_HPP_ +#ifndef HUSARION_UGV_GAZEBO_GUI_E_STOP_HPP_ +#define HUSARION_UGV_GAZEBO_GUI_E_STOP_HPP_ #include @@ -23,7 +23,7 @@ #include -namespace panther_gazebo +namespace husarion_ugv_gazebo { class EStop : public ignition::gui::Plugin @@ -59,6 +59,6 @@ protected slots: rclcpp::Client::SharedPtr e_stop_reset_client_; rclcpp::Client::SharedPtr e_stop_trigger_client_; }; -} // namespace panther_gazebo +} // namespace husarion_ugv_gazebo -#endif // PANTHER_GAZEBO_GUI_E_STOP_HPP_ +#endif // HUSARION_UGV_GAZEBO_GUI_E_STOP_HPP_ diff --git a/panther_gazebo/include/panther_gazebo/led_strip.hpp b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/led_strip.hpp similarity index 96% rename from panther_gazebo/include/panther_gazebo/led_strip.hpp rename to husarion_ugv_gazebo/include/husarion_ugv_gazebo/led_strip.hpp index 0f0945ea..74d76444 100644 --- a/panther_gazebo/include/panther_gazebo/led_strip.hpp +++ b/husarion_ugv_gazebo/include/husarion_ugv_gazebo/led_strip.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_GAZEBO_LED_STRIP_HPP_ -#define PANTHER_GAZEBO_LED_STRIP_HPP_ +#ifndef HUSARION_UGV_GAZEBO_LED_STRIP_HPP_ +#define HUSARION_UGV_GAZEBO_LED_STRIP_HPP_ #include #include @@ -33,7 +33,7 @@ #include #include -namespace panther_gazebo +namespace husarion_ugv_gazebo { /** @@ -144,6 +144,6 @@ class LEDStrip : public gz::sim::System, 1)}; // Avoid initialization errors when the robot is not yet spawned on the scene. }; -} // namespace panther_gazebo +} // namespace husarion_ugv_gazebo -#endif // PANTHER_GAZEBO_LED_STRIP_HPP_ +#endif // HUSARION_UGV_GAZEBO_LED_STRIP_HPP_ diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/husarion_ugv_gazebo/launch/simulate_robot.launch.py similarity index 60% rename from panther_gazebo/launch/simulate_robot.launch.py rename to husarion_ugv_gazebo/launch/simulate_robot.launch.py index ad7252e7..a13a0115 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/husarion_ugv_gazebo/launch/simulate_robot.launch.py @@ -14,6 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition @@ -30,6 +31,16 @@ def generate_launch_description(): + add_world_transform = LaunchConfiguration("add_world_transform") + declare_add_world_transform_arg = DeclareLaunchArgument( + "add_world_transform", + default_value="False", + description=( + "Adds a world frame that connects the tf trees of individual robots (useful when running" + " multiple robots)." + ), + choices=["True", "true", "False", "false"], + ) declare_battery_config_path_arg = DeclareLaunchArgument( "battery_config_path", @@ -38,29 +49,39 @@ def generate_launch_description(): "This configuration is intended for use in simulations only." ), default_value=PathJoinSubstitution( - [FindPackageShare("panther_gazebo"), "config", "battery_plugin_config.yaml"] + [FindPackageShare("husarion_ugv_gazebo"), "config", "battery_plugin.yaml"] ), ) components_config_path = LaunchConfiguration("components_config_path") + robot_model = LaunchConfiguration("robot_model") + robot_description_pkg = PythonExpression(["'", robot_model, "_description'"]) declare_components_config_path_arg = DeclareLaunchArgument( "components_config_path", default_value=PathJoinSubstitution( - [FindPackageShare("panther_description"), "config", "components.yaml"] + [FindPackageShare(robot_description_pkg), "config", "components.yaml"] ), description=( "Additional components configuration file. Components described in this file " - "are dynamically included in Panther's urdf." - "Panther options are described here " + "are dynamically included in robot's URDF." + "Available options are described in the manual: " "https://husarion.com/manuals/panther/panther-options/" ), ) + disable_manager = LaunchConfiguration("disable_manager") + declare_disable_manager_arg = DeclareLaunchArgument( + "disable_manager", + default_value="False", + description="Enable or disable manager_bt_node.", + choices=["True", "true", "False", "false"], + ) + gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path") declare_gz_bridge_config_path_arg = DeclareLaunchArgument( "gz_bridge_config_path", default_value=PathJoinSubstitution( - [FindPackageShare("panther_gazebo"), "config", "gz_bridge.yaml"] + [FindPackageShare("husarion_ugv_gazebo"), "config", "gz_bridge.yaml"] ), description="Path to the parameter_bridge configuration file.", ) @@ -72,31 +93,29 @@ def generate_launch_description(): description="Add namespace to all launched nodes.", ) - use_ekf = LaunchConfiguration("use_ekf") - declare_use_ekf_arg = DeclareLaunchArgument( - "use_ekf", - default_value="True", - description="Enable or disable EKF.", - choices=["True", "true", "False", "false"], + declare_robot_model_arg = DeclareLaunchArgument( + "robot_model", + default_value=EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther"), + description="Specify robot model", + choices=["lynx", "panther"], ) spawn_robot_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_gazebo"), "launch", "spawn_robot.launch.py"] + [FindPackageShare("husarion_ugv_gazebo"), "launch", "spawn_robot.launch.py"] ) ), launch_arguments={ "add_wheel_joints": "False", "namespace": namespace, - "use_sim": "True", }.items(), ) lights_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_lights"), "launch", "lights.launch.py"] + [FindPackageShare("husarion_ugv_lights"), "launch", "lights.launch.py"] ) ), launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), @@ -105,27 +124,18 @@ def generate_launch_description(): manager_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_manager"), "launch", "manager.launch.py"] + [FindPackageShare("husarion_ugv_manager"), "launch", "manager.launch.py"] ) ), launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), - ) - - gz_led_strip_config = PathJoinSubstitution( - [FindPackageShare("panther_gazebo"), "config", "led_strips.yaml"] - ) - - gz_led_strip_config = ReplaceString( - source_file=gz_led_strip_config, - replacements={"parent_link: panther": ["parent_link: ", namespace]}, - condition=UnlessCondition(PythonExpression(["'", namespace, "' == ''"])), + condition=UnlessCondition(disable_manager), ) controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("panther_controller"), + FindPackageShare("husarion_ugv_controller"), "launch", "controller.launch.py", ] @@ -142,14 +152,13 @@ def generate_launch_description(): PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("panther_localization"), + FindPackageShare("husarion_ugv_localization"), "launch", "localization.launch.py", ] ) ), launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), - condition=IfCondition(use_ekf), ) simulate_components = IncludeLaunchDescription( @@ -179,26 +188,58 @@ def generate_launch_description(): gz_bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", - name="panther_base_gz_bridge", + name="gz_bridge", parameters=[{"config_file": namespaced_gz_bridge_config_path}], namespace=namespace, emulate_tty=True, ) - return LaunchDescription( - [ - declare_battery_config_path_arg, - declare_components_config_path_arg, - declare_gz_bridge_config_path_arg, - declare_namespace_arg, - declare_use_ekf_arg, - SetUseSimTime(True), - spawn_robot_launch, - lights_launch, - manager_launch, - controller_launch, - ekf_launch, - simulate_components, - gz_bridge, - ] - ) + child_tf = PythonExpression(["'", namespace, "' + '/odom' if '", namespace, "' else 'odom'"]) + + world_transform = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_tf_publisher", + arguments=[ + "--x", + LaunchConfiguration("x"), + "--y", + LaunchConfiguration("y"), + "--z", + LaunchConfiguration("z"), + "--roll", + LaunchConfiguration("roll"), + "--pitch", + LaunchConfiguration("pitch"), + "--yaw", + LaunchConfiguration("yaw"), + "--frame-id", + "world", + "--child-frame-id", + child_tf, + ], + namespace=namespace, + emulate_tty=True, + condition=IfCondition(add_world_transform), + ) + + actions = [ + declare_add_world_transform_arg, + declare_battery_config_path_arg, + declare_robot_model_arg, # robot_model is used by components_config_path + declare_components_config_path_arg, + declare_disable_manager_arg, + declare_gz_bridge_config_path_arg, + declare_namespace_arg, + SetUseSimTime(True), + spawn_robot_launch, + lights_launch, + manager_launch, + controller_launch, + ekf_launch, + simulate_components, + gz_bridge, + world_transform, + ] + + return LaunchDescription(actions) diff --git a/panther_gazebo/launch/simulation.launch.py b/husarion_ugv_gazebo/launch/simulation.launch.py similarity index 65% rename from panther_gazebo/launch/simulation.launch.py rename to husarion_ugv_gazebo/launch/simulation.launch.py index 04c192d7..34da7dc9 100644 --- a/panther_gazebo/launch/simulation.launch.py +++ b/husarion_ugv_gazebo/launch/simulation.launch.py @@ -16,6 +16,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( EnvironmentVariable, @@ -33,7 +34,7 @@ def generate_launch_description(): declare_gz_gui = DeclareLaunchArgument( "gz_gui", default_value=PathJoinSubstitution( - [FindPackageShare("panther_gazebo"), "config", "teleop_with_estop.config"] + [FindPackageShare("husarion_ugv_gazebo"), "config", "teleop_with_estop.config"] ), description="Run simulation with specific GUI layout.", ) @@ -45,6 +46,14 @@ def generate_launch_description(): description="Add namespace to all launched nodes.", ) + use_rviz = LaunchConfiguration("use_rviz") + declare_use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="True", + description="Run RViz simultaneously.", + choices=["True", "true", "False", "false"], + ) + namespaced_gz_gui = ReplaceString( source_file=gz_gui, replacements={"{namespace}": namespace}, @@ -59,25 +68,40 @@ def generate_launch_description(): launch_arguments={"gz_gui": namespaced_gz_gui, "gz_log_level": "1"}.items(), ) - simulate_robots = IncludeLaunchDescription( + rviz_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("panther_gazebo"), + FindPackageShare("panther_description"), "launch", - "simulate_multiple_robots.launch.py", + "rviz.launch.py", ] ) ), + condition=IfCondition(use_rviz), ) - return LaunchDescription( - [ - declare_gz_gui, - declare_namespace_arg, - # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo) - SetUseSimTime(True), - gz_sim, - simulate_robots, - ] + simulate_robots = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("husarion_ugv_gazebo"), + "launch", + "simulate_robot.launch.py", + ] + ) + ), ) + + actions = [ + declare_gz_gui, + declare_namespace_arg, + declare_use_rviz_arg, + # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo) + SetUseSimTime(True), + gz_sim, + rviz_launch, + simulate_robots, + ] + + return LaunchDescription(actions) diff --git a/panther_gazebo/launch/spawn_robot.launch.py b/husarion_ugv_gazebo/launch/spawn_robot.launch.py similarity index 74% rename from panther_gazebo/launch/spawn_robot.launch.py rename to husarion_ugv_gazebo/launch/spawn_robot.launch.py index 42e3e27e..f3013e13 100644 --- a/panther_gazebo/launch/spawn_robot.launch.py +++ b/husarion_ugv_gazebo/launch/spawn_robot.launch.py @@ -14,6 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. + +from husarion_ugv_utils.messages import welcome_msg from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -21,10 +23,10 @@ EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution, + PythonExpression, ) from launch_ros.actions import Node, SetUseSimTime from launch_ros.substitutions import FindPackageShare -from panther_utils.welcomeMsg import welcomeMsg def generate_launch_description(): @@ -36,6 +38,14 @@ def generate_launch_description(): description="Add namespace to all launched nodes.", ) + robot_model = LaunchConfiguration("robot_model") + declare_robot_model_arg = DeclareLaunchArgument( + "robot_model", + default_value=EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther"), + description="Specify robot model", + choices=["lynx", "panther"], + ) + x = LaunchConfiguration("x") declare_x_arg = DeclareLaunchArgument( "x", default_value="0.0", description="Initial robot position in the global 'x' axis." @@ -48,7 +58,7 @@ def generate_launch_description(): z = LaunchConfiguration("z") declare_z_arg = DeclareLaunchArgument( - "z", default_value="0.2", description="Initial robot position in the global 'z' axis." + "z", default_value="0.0", description="Initial robot position in the global 'z' axis." ) roll = LaunchConfiguration("roll") @@ -70,17 +80,17 @@ def generate_launch_description(): "Robot namespace": namespace, "Initial pose": ["(", x, ", ", y, ", ", z, ", ", roll, ", ", pitch, ", ", yaw, ")"], } - welcome_msg = welcomeMsg("---", "simulation", log_stats) + welcome_info = welcome_msg(robot_model, "----", "simulation", log_stats) - add_wheel_joints = LaunchConfiguration("add_wheel_joints", default="True") + robot_description_pkg = PythonExpression(["'", robot_model, "_description'"]) load_urdf = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_description"), "launch", "load_urdf.launch.py"] + [FindPackageShare(robot_description_pkg), "launch", "load_urdf.launch.py"] ) ), launch_arguments={ - "add_wheel_joints": add_wheel_joints, + "add_wheel_joints": LaunchConfiguration("add_wheel_joints", default="True"), "namespace": namespace, "use_sim": "True", }.items(), @@ -111,18 +121,19 @@ def generate_launch_description(): emulate_tty=True, ) - return LaunchDescription( - [ - declare_namespace_arg, - declare_x_arg, - declare_y_arg, - declare_z_arg, - declare_roll_arg, - declare_pitch_arg, - declare_yaw_arg, - SetUseSimTime(True), - welcome_msg, - load_urdf, - spawn_robot, - ] - ) + actions = [ + declare_namespace_arg, + declare_robot_model_arg, + declare_x_arg, + declare_y_arg, + declare_z_arg, + declare_roll_arg, + declare_pitch_arg, + declare_yaw_arg, + SetUseSimTime(True), + welcome_info, + load_urdf, + spawn_robot, + ] + + return LaunchDescription(actions) diff --git a/panther_gazebo/package.xml b/husarion_ugv_gazebo/package.xml similarity index 88% rename from panther_gazebo/package.xml rename to husarion_ugv_gazebo/package.xml index 7be9ad3b..4ab4e94a 100644 --- a/panther_gazebo/package.xml +++ b/husarion_ugv_gazebo/package.xml @@ -1,9 +1,9 @@ - panther_gazebo + husarion_ugv_gazebo 2.1.2 - The panther_description package + The packages simulating behaviour of the Husarion robots Husarion Apache License 2.0 @@ -29,15 +29,16 @@ controller_manager husarion_gz_worlds + husarion_ugv_controller + husarion_ugv_lights + husarion_ugv_localization + husarion_ugv_manager + husarion_ugv_utils launch launch_ros + lynx_description nav2_common - panther_controller panther_description - panther_lights - panther_localization - panther_manager - panther_utils robot_state_publisher ros_components_description ros_gz_bridge diff --git a/panther_gazebo/panther_simulation_plugins.xml b/husarion_ugv_gazebo/plugins.xml similarity index 62% rename from panther_gazebo/panther_simulation_plugins.xml rename to husarion_ugv_gazebo/plugins.xml index 7a0aef76..f97c60b1 100644 --- a/panther_gazebo/panther_simulation_plugins.xml +++ b/husarion_ugv_gazebo/plugins.xml @@ -1,7 +1,7 @@ - + Control joints position with E-Stop functionalities in Gazebo using ROS2. diff --git a/panther_gazebo/src/gz_panther_system.cpp b/husarion_ugv_gazebo/src/estop_system.cpp similarity index 82% rename from panther_gazebo/src/gz_panther_system.cpp rename to husarion_ugv_gazebo/src/estop_system.cpp index d705d716..4f37923f 100644 --- a/panther_gazebo/src/gz_panther_system.cpp +++ b/husarion_ugv_gazebo/src/estop_system.cpp @@ -28,7 +28,7 @@ #include #include -#include "panther_gazebo/gz_panther_system.hpp" +#include "husarion_ugv_gazebo/estop_system.hpp" struct jointData { @@ -94,10 +94,10 @@ class ign_ros2_control::IgnitionSystemPrivate double position_proportional_gain_; }; -namespace panther_gazebo +namespace husarion_ugv_gazebo { -CallbackReturn GzPantherSystem::on_init(const hardware_interface::HardwareInfo & system_info) +CallbackReturn EStopSystem::on_init(const hardware_interface::HardwareInfo & system_info) { if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; @@ -108,24 +108,24 @@ CallbackReturn GzPantherSystem::on_init(const hardware_interface::HardwareInfo & return CallbackReturn::SUCCESS; } -CallbackReturn GzPantherSystem::on_configure(const rclcpp_lifecycle::State & previous_state) +CallbackReturn EStopSystem::on_configure(const rclcpp_lifecycle::State & previous_state) { SetupEStop(); return hardware_interface::SystemInterface::on_configure(previous_state); } -CallbackReturn GzPantherSystem::on_activate(const rclcpp_lifecycle::State & previous_state) +CallbackReturn EStopSystem::on_activate(const rclcpp_lifecycle::State & previous_state) { PublishEStopStatus(); return hardware_interface::SystemInterface::on_activate(previous_state); } -CallbackReturn GzPantherSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state) +CallbackReturn EStopSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state) { return hardware_interface::SystemInterface::on_deactivate(previous_state); } -hardware_interface::return_type GzPantherSystem::write( +hardware_interface::return_type EStopSystem::write( const rclcpp::Time & time, const rclcpp::Duration & period) { if (e_stop_active_) { @@ -135,7 +135,7 @@ hardware_interface::return_type GzPantherSystem::write( return IgnitionSystem::write(time, period); } -void GzPantherSystem::SetupEStop() +void EStopSystem::SetupEStop() { e_stop_publisher_ = nh_->create_publisher( "~/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -143,22 +143,22 @@ void GzPantherSystem::SetupEStop() e_stop_reset_service_ = nh_->create_service( "~/e_stop_reset", std::bind( - &GzPantherSystem::EStopResetCallback, this, std::placeholders::_1, std::placeholders::_2)); + &EStopSystem::EStopResetCallback, this, std::placeholders::_1, std::placeholders::_2)); e_stop_trigger_service_ = nh_->create_service( "~/e_stop_trigger", std::bind( - &GzPantherSystem::EStopTriggerCallback, this, std::placeholders::_1, std::placeholders::_2)); + &EStopSystem::EStopTriggerCallback, this, std::placeholders::_1, std::placeholders::_2)); } -void GzPantherSystem::PublishEStopStatus() +void EStopSystem::PublishEStopStatus() { std_msgs::msg::Bool e_stop_msg; e_stop_msg.data = e_stop_active_; e_stop_publisher_->publish(e_stop_msg); } -void GzPantherSystem::EStopResetCallback( +void EStopSystem::EStopResetCallback( const TriggerSrv::Request::SharedPtr & /*request*/, TriggerSrv::Response::SharedPtr response) { e_stop_active_ = false; @@ -167,7 +167,7 @@ void GzPantherSystem::EStopResetCallback( PublishEStopStatus(); } -void GzPantherSystem::EStopTriggerCallback( +void EStopSystem::EStopTriggerCallback( const TriggerSrv::Request::SharedPtr & /*request*/, TriggerSrv::Response::SharedPtr response) { e_stop_active_ = true; @@ -176,7 +176,7 @@ void GzPantherSystem::EStopTriggerCallback( PublishEStopStatus(); } -} // namespace panther_gazebo +} // namespace husarion_ugv_gazebo #include "pluginlib/class_list_macros.hpp" // NOLINT -PLUGINLIB_EXPORT_CLASS(panther_gazebo::GzPantherSystem, ign_ros2_control::IgnitionSystemInterface) +PLUGINLIB_EXPORT_CLASS(husarion_ugv_gazebo::EStopSystem, ign_ros2_control::IgnitionSystemInterface) diff --git a/panther_gazebo/src/gui/EStop.qml b/husarion_ugv_gazebo/src/gui/EStop.qml similarity index 100% rename from panther_gazebo/src/gui/EStop.qml rename to husarion_ugv_gazebo/src/gui/EStop.qml diff --git a/panther_gazebo/src/gui/EStop.qrc b/husarion_ugv_gazebo/src/gui/EStop.qrc similarity index 100% rename from panther_gazebo/src/gui/EStop.qrc rename to husarion_ugv_gazebo/src/gui/EStop.qrc diff --git a/panther_gazebo/src/gui/e_stop.cpp b/husarion_ugv_gazebo/src/gui/e_stop.cpp similarity index 94% rename from panther_gazebo/src/gui/e_stop.cpp rename to husarion_ugv_gazebo/src/gui/e_stop.cpp index ff4c3010..ffad684d 100644 --- a/panther_gazebo/src/gui/e_stop.cpp +++ b/husarion_ugv_gazebo/src/gui/e_stop.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_gazebo/gui/e_stop.hpp" +#include "husarion_ugv_gazebo/gui/e_stop.hpp" #include @@ -23,7 +23,7 @@ #include -namespace panther_gazebo +namespace husarion_ugv_gazebo { EStop::EStop() : ignition::gui::Plugin() { rclcpp::init(0, nullptr); } @@ -90,6 +90,6 @@ void EStop::SetNamespace(const QString & ns) ignmsg << "Changed namespace to: " << namespace_ << std::endl; } -} // namespace panther_gazebo +} // namespace husarion_ugv_gazebo -IGNITION_ADD_PLUGIN(panther_gazebo::EStop, ignition::gui::Plugin) +IGNITION_ADD_PLUGIN(husarion_ugv_gazebo::EStop, ignition::gui::Plugin) diff --git a/panther_gazebo/src/led_strip.cpp b/husarion_ugv_gazebo/src/led_strip.cpp similarity index 97% rename from panther_gazebo/src/led_strip.cpp rename to husarion_ugv_gazebo/src/led_strip.cpp index c7820bb1..c0984620 100644 --- a/panther_gazebo/src/led_strip.cpp +++ b/husarion_ugv_gazebo/src/led_strip.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_gazebo/led_strip.hpp" +#include "husarion_ugv_gazebo/led_strip.hpp" #include #include @@ -25,7 +25,7 @@ #include #include -namespace panther_gazebo +namespace husarion_ugv_gazebo { void LEDStrip::Configure( const gz::sim::Entity & entity, const std::shared_ptr & sdf, @@ -275,8 +275,8 @@ void LEDStrip::CreateMarker( node_.Request("/marker", marker_msg); } -} // namespace panther_gazebo +} // namespace husarion_ugv_gazebo IGNITION_ADD_PLUGIN( - panther_gazebo::LEDStrip, gz::sim::System, panther_gazebo::LEDStrip::ISystemConfigure, - panther_gazebo::LEDStrip::ISystemPreUpdate) + husarion_ugv_gazebo::LEDStrip, gz::sim::System, husarion_ugv_gazebo::LEDStrip::ISystemConfigure, + husarion_ugv_gazebo::LEDStrip::ISystemPreUpdate) diff --git a/panther_hardware_interfaces/CHANGELOG.rst b/husarion_ugv_hardware_interfaces/CHANGELOG.rst similarity index 100% rename from panther_hardware_interfaces/CHANGELOG.rst rename to husarion_ugv_hardware_interfaces/CHANGELOG.rst diff --git a/husarion_ugv_hardware_interfaces/CMakeLists.txt b/husarion_ugv_hardware_interfaces/CMakeLists.txt new file mode 100644 index 00000000..66d2aeff --- /dev/null +++ b/husarion_ugv_hardware_interfaces/CMakeLists.txt @@ -0,0 +1,349 @@ +cmake_minimum_required(VERSION 3.10.2) + +# Handle superbuild first +option(USE_SUPERBUILD "Whether or not a superbuild should be invoked" ON) + +if(USE_SUPERBUILD) + project(SUPERBUILD NONE) + include(cmake/SuperBuild.cmake) + return() +else() + project(husarion_ugv_hardware_interfaces) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(PACKAGE_DEPENDENCIES + ament_cmake + controller_interface + diagnostic_updater + generate_parameter_library + geometry_msgs + hardware_interface + imu_filter_madgwick + panther_msgs + husarion_ugv_utils + phidgets_api + PkgConfig + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2_geometry_msgs + tf2_ros) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +include_directories(include) + +generate_parameter_library(phidgets_spatial_parameters + config/phidgets_spatial_parameters.yaml) + +set(ENV{PKG_CONFIG_PATH} + "${CMAKE_INSTALL_PREFIX}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}") + +pkg_check_modules(LIBLELY_COAPP REQUIRED IMPORTED_TARGET liblely-coapp) +pkg_check_modules(LIBGPIOD REQUIRED IMPORTED_TARGET libgpiodcxx) + +add_library( + ${PROJECT_NAME} SHARED + src/phidget_imu_sensor/phidget_imu_sensor.cpp + src/robot_system/gpio/gpio_controller.cpp + src/robot_system/gpio/gpio_driver.cpp + src/robot_system/robot_driver/canopen_manager.cpp + src/robot_system/robot_driver/roboteq_data_converters.cpp + src/robot_system/robot_driver/roboteq_driver.cpp + src/robot_system/robot_driver/roboteq_error_filter.cpp + src/robot_system/robot_driver/roboteq_robot_driver.cpp + src/robot_system/robot_driver/lynx_robot_driver.cpp + src/robot_system/robot_driver/panther_robot_driver.cpp + src/robot_system/system_e_stop.cpp + src/robot_system/system_ros_interface.cpp + src/robot_system/lynx_system.cpp + src/robot_system/panther_system.cpp + src/robot_system/ugv_system.cpp + src/utils.cpp) +ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_DEPENDENCIES}) +target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP + PkgConfig::LIBGPIOD phidgets_spatial_parameters) + +target_compile_definitions( + ${PROJECT_NAME} PRIVATE "HUSARION_UGV_HARDWARE_INTERFACES_BUILDING_DLL") + +pluginlib_export_plugin_description_file(hardware_interface + husarion_ugv_hardware_interfaces.xml) + +install( + TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(ros_testing REQUIRED) + find_package(husarion_ugv_utils REQUIRED) + + install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test) + + ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp src/utils.cpp) + + ament_add_gtest(${PROJECT_NAME}_test_phidget_imu_sensor + test/phidget_imu_sensor/test_phidget_imu_sensor.cpp) + ament_target_dependencies( + ${PROJECT_NAME}_test_phidget_imu_sensor hardware_interface rclcpp + husarion_ugv_utils panther_msgs phidgets_api) + target_link_libraries(${PROJECT_NAME}_test_phidget_imu_sensor ${PROJECT_NAME} + phidgets_spatial_parameters) + + ament_add_gtest( + ${PROJECT_NAME}_test_roboteq_error_filter + test/unit/robot_system/robot_driver/test_roboteq_error_filter.cpp + src/robot_system/robot_driver/roboteq_error_filter.cpp) + + ament_add_gtest( + ${PROJECT_NAME}_test_roboteq_data_converters + test/unit/robot_system/robot_driver/test_roboteq_data_converters.cpp + src/robot_system/robot_driver/roboteq_data_converters.cpp src/utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_roboteq_data_converters + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_roboteq_data_converters + panther_msgs husarion_ugv_utils) + target_link_libraries(${PROJECT_NAME}_test_roboteq_data_converters + PkgConfig::LIBLELY_COAPP) + + ament_add_gtest( + ${PROJECT_NAME}_test_canopen_manager + test/unit/robot_system/robot_driver/test_canopen_manager.cpp + src/robot_system/robot_driver/canopen_manager.cpp src/utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_canopen_manager + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_canopen_manager rclcpp + panther_msgs husarion_ugv_utils) + target_link_libraries(${PROJECT_NAME}_test_canopen_manager + PkgConfig::LIBLELY_COAPP) + + ament_add_gtest( + ${PROJECT_NAME}_test_roboteq_driver + test/unit/robot_system/robot_driver/test_roboteq_driver.cpp + src/robot_system/robot_driver/canopen_manager.cpp + src/robot_system/robot_driver/roboteq_driver.cpp + src/utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_roboteq_driver + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_roboteq_driver rclcpp + panther_msgs husarion_ugv_utils) + target_link_libraries(${PROJECT_NAME}_test_roboteq_driver + PkgConfig::LIBLELY_COAPP) + + ament_add_gmock( + ${PROJECT_NAME}_test_roboteq_robot_driver + test/unit/robot_system/robot_driver/test_roboteq_robot_driver.cpp + src/robot_system/robot_driver/canopen_manager.cpp + src/robot_system/robot_driver/roboteq_driver.cpp + src/robot_system/robot_driver/roboteq_data_converters.cpp + src/robot_system/robot_driver/roboteq_robot_driver.cpp + src/utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_roboteq_robot_driver + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_roboteq_robot_driver rclcpp + panther_msgs husarion_ugv_utils) + target_link_libraries(${PROJECT_NAME}_test_roboteq_robot_driver + PkgConfig::LIBLELY_COAPP) + + ament_add_gmock( + ${PROJECT_NAME}_test_lynx_robot_driver + test/unit/robot_system/robot_driver/test_lynx_robot_driver.cpp + src/robot_system/robot_driver/canopen_manager.cpp + src/robot_system/robot_driver/roboteq_driver.cpp + src/robot_system/robot_driver/roboteq_data_converters.cpp + src/robot_system/robot_driver/roboteq_robot_driver.cpp + src/robot_system/robot_driver/lynx_robot_driver.cpp + src/utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_lynx_robot_driver + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_lynx_robot_driver rclcpp + panther_msgs husarion_ugv_utils) + target_link_libraries(${PROJECT_NAME}_test_lynx_robot_driver + PkgConfig::LIBLELY_COAPP) + + ament_add_gmock( + ${PROJECT_NAME}_test_panther_robot_driver + test/unit/robot_system/robot_driver/test_panther_robot_driver.cpp + src/robot_system/robot_driver/canopen_manager.cpp + src/robot_system/robot_driver/roboteq_driver.cpp + src/robot_system/robot_driver/roboteq_data_converters.cpp + src/robot_system/robot_driver/roboteq_robot_driver.cpp + src/robot_system/robot_driver/panther_robot_driver.cpp + src/utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_panther_robot_driver + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_panther_robot_driver rclcpp + panther_msgs husarion_ugv_utils) + target_link_libraries(${PROJECT_NAME}_test_panther_robot_driver + PkgConfig::LIBLELY_COAPP) + + ament_add_gmock( + ${PROJECT_NAME}_test_gpiod_controller + test/robot_system/gpio/test_gpio_controller.cpp + src/robot_system/gpio/gpio_controller.cpp + src/robot_system/gpio/gpio_driver.cpp) + ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller + husarion_ugv_utils) + target_link_libraries(${PROJECT_NAME}_test_gpiod_controller + PkgConfig::LIBGPIOD) + + ament_add_gtest( + ${PROJECT_NAME}_test_system_ros_interface + test/unit/robot_system/test_system_ros_interface.cpp + src/robot_system/system_ros_interface.cpp + src/robot_system/robot_driver/roboteq_data_converters.cpp + src/robot_system/gpio/gpio_controller.cpp + src/utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_system_ros_interface + PUBLIC $ + $) + ament_target_dependencies( + ${PROJECT_NAME}_test_system_ros_interface + diagnostic_updater + rclcpp + panther_msgs + husarion_ugv_utils + realtime_tools + std_srvs) + target_link_libraries(${PROJECT_NAME}_test_system_ros_interface + PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD) + + ament_add_gmock( + ${PROJECT_NAME}_test_ugv_system + test/unit/robot_system/test_ugv_system.cpp + src/robot_system/gpio/gpio_controller.cpp + src/robot_system/gpio/gpio_driver.cpp + src/robot_system/system_e_stop.cpp + src/robot_system/robot_driver/roboteq_data_converters.cpp + src/robot_system/robot_driver/roboteq_error_filter.cpp + src/robot_system/system_ros_interface.cpp + src/robot_system/ugv_system.cpp + src/utils.cpp) + set_tests_properties(${PROJECT_NAME}_test_ugv_system PROPERTIES TIMEOUT 120) + target_include_directories( + ${PROJECT_NAME}_test_ugv_system + PUBLIC $ + $) + ament_target_dependencies( + ${PROJECT_NAME}_test_ugv_system + diagnostic_updater + hardware_interface + rclcpp + panther_msgs + husarion_ugv_utils + std_msgs + std_srvs) + target_link_libraries(${PROJECT_NAME}_test_ugv_system + PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD) + + ament_add_gmock( + ${PROJECT_NAME}_test_lynx_system + test/unit/robot_system/test_lynx_system.cpp + src/robot_system/gpio/gpio_controller.cpp + src/robot_system/gpio/gpio_driver.cpp + src/robot_system/system_e_stop.cpp + src/robot_system/robot_driver/canopen_manager.cpp + src/robot_system/robot_driver/roboteq_robot_driver.cpp + src/robot_system/robot_driver/roboteq_data_converters.cpp + src/robot_system/robot_driver/roboteq_driver.cpp + src/robot_system/robot_driver/roboteq_error_filter.cpp + src/robot_system/robot_driver/lynx_robot_driver.cpp + src/robot_system/system_ros_interface.cpp + src/robot_system/lynx_system.cpp + src/robot_system/ugv_system.cpp + src/utils.cpp) + set_tests_properties(${PROJECT_NAME}_test_lynx_system PROPERTIES TIMEOUT 120) + target_include_directories( + ${PROJECT_NAME}_test_lynx_system + PUBLIC $ + $) + ament_target_dependencies( + ${PROJECT_NAME}_test_lynx_system + diagnostic_updater + hardware_interface + rclcpp + panther_msgs + husarion_ugv_utils + std_msgs + std_srvs) + target_link_libraries(${PROJECT_NAME}_test_lynx_system + PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD) + + ament_add_gmock( + ${PROJECT_NAME}_test_panther_system + test/unit/robot_system/test_panther_system.cpp + src/robot_system/gpio/gpio_controller.cpp + src/robot_system/gpio/gpio_driver.cpp + src/robot_system/system_e_stop.cpp + src/robot_system/robot_driver/canopen_manager.cpp + src/robot_system/robot_driver/roboteq_robot_driver.cpp + src/robot_system/robot_driver/roboteq_data_converters.cpp + src/robot_system/robot_driver/roboteq_driver.cpp + src/robot_system/robot_driver/roboteq_error_filter.cpp + src/robot_system/robot_driver/panther_robot_driver.cpp + src/robot_system/system_ros_interface.cpp + src/robot_system/panther_system.cpp + src/robot_system/ugv_system.cpp + src/utils.cpp) + set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT + 120) + target_include_directories( + ${PROJECT_NAME}_test_panther_system + PUBLIC $ + $) + ament_target_dependencies( + ${PROJECT_NAME}_test_panther_system + diagnostic_updater + hardware_interface + rclcpp + panther_msgs + husarion_ugv_utils + std_msgs + std_srvs) + target_link_libraries(${PROJECT_NAME}_test_panther_system + PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD) + + # Integration tests option(TEST_INTEGRATION "Run integration tests" OFF) + if(TEST_INTEGRATION) # Hardware integration + ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver + test/robot_system/gpio/test_gpio_driver.cpp) + ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver + husarion_ugv_utils) + target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME} + PkgConfig::LIBGPIOD) + endif() + +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${PACKAGE_DEPENDENCIES}) + +ament_package() diff --git a/panther_hardware_interfaces/CODE_STRUCTURE.md b/husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md similarity index 59% rename from panther_hardware_interfaces/CODE_STRUCTURE.md rename to husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md index d7a448d4..a2e2b9a1 100644 --- a/panther_hardware_interfaces/CODE_STRUCTURE.md +++ b/husarion_ugv_hardware_interfaces/CODE_STRUCTURE.md @@ -1,21 +1,37 @@ # Structure -A brief introduction to the code structure of the Panther system. +A brief introduction to the code structure of the Husarion UGV system. -## RoboteqDriver +## DriverInterface -Low-level CANopen driver implementing LoopDriver from [Lely](https://opensource.lely.com/canopen/). +Interface to manage robot driver. +Implementations: + +* `RoboteqDriver`: Low-level CANopen driver implementing LoopDriver from [Lely](https://opensource.lely.com/canopen/). It takes care of translating CANopen indexes into meaningful data. Provided methods can be used for sending commands and reading all the useful parameters from the Roboteq drivers (they abstract low level SDO and PDO communication). -Timestamp of all received PDO data is also saved, which can be later used for detecting timeout errors. +Timestamp of all received PDO data is also saved, which can be later used for detecting timeout errors. Also, manages `MotorDrivers`. + +## MotorDriverInterface + +Abstract interface for managing each motor connected to the driver. +Implementations: + +* `RoboteqMotorDriver`: Responsible for reading state and sending command velocities with usage of `RoboteqDriver` interfaces. + +## CANopenManager -## CANopenController +Takes care of CANopen communication - creates and initializes master controller. For handling CANopen communication separate thread is created with configurable RT priority. -Takes care of CANopen communication - creates and initializes master controller and two Roboteq drivers (front and rear). For handling CANopen communication separate thread is created with configurable RT priority (additionally two threads for each driver are also created). +## RobotDriver -## MotorsController +Interface to control robot drivers. +Implementations: -This class abstracts the usage of two Roboteq controllers. It uses canopen_controller for communication with Roboteq controllers, implements the activation procedure for controllers (resets script and sends initial 0 command), and provides methods to get data feedback and send commands. Data is converted between raw Roboteq formats and SI units using `roboteq_data_converters`. +* `RoboteqRobotDriver`: This class abstracts the usage of Roboteq controllers. It uses canopen_controller for communication with Roboteq controllers, implements the activation procedure for controllers (resets script and sends initial 0 command), and provides methods to get data feedback and send commands. Data is converted between raw Roboteq formats and SI units using `roboteq_data_converters`. +It has two concrete implementations: + * `LynxRobotDriver`: Contains one Roboteq controller. + * `PantherRobotDriver`: Contains two Roboteq controllers. ## RoboteqDataConverters @@ -27,14 +43,14 @@ Data feedback converters also store data (it is passed using Set methods, and la * `MotorState` - converts position, velocity and torque feedback * `FaultFlag`, `ScriptFlag`, `RuntimeError` - converts flag error data into messages -* `DriverState` - temperature, voltage, and current +* `RoboteqDriverState` - temperature, voltage, and current -Feedback converters are combined in the `RoboteqData` class to provide the full state of one controller. It consists of +Feedback converters are combined in the `DriverData` class to provide the full state of one controller. It consists of * 2 `MotorState` (left and right) * `FaultFlag`, `ScriptFlag` * 2 `RuntimeError` (for left and right motors) -* `DriverState` +* `RoboteqDriverState` ## RoboteqErrorFilter @@ -44,30 +60,28 @@ As they usually are rare and singular occurrences, it is better to filter some o ## GPIODriver The GPIODriver is a low-level class responsible for direct interaction with the GPIO (General Purpose Input/Output) pins on the Raspberry Pi. -It comprises a wrapper implementation for the GPIOD library, enabling real-time manipulation of GPIO pins on the Raspberry Pi. Offering convenient interfaces for setting pin values, altering their direction, monitoring events, and conducting other GPIO operations, this library facilitates effective GPIO pin management on the Panther robot. It simplifies integration within robotic applications. +It comprises a wrapper implementation for the GPIOD library, enabling real-time manipulation of GPIO pins on the Raspberry Pi. Offering convenient interfaces for setting pin values, altering their direction, monitoring events, and conducting other GPIO operations, this library facilitates effective GPIO pin management on the Husarion UGV. It simplifies integration within robotic applications. ## GPIOController The GPIOController provides wrappers for the GPIO driver, handling reading and writing pins of the RPi GPIO. It includes the following utilities: * `GPIOControllerInterface`: Interface for all wrappers that handle GPIO control tasks. -* `GPIOControllerPTH12X`: Class with specific logic for the Panther robot with version 1.20 and above. -* `GPIOControllerPTH10X`: Class with specific logic for the Panther robot with version below 1.20. -* `Watchdog`: Entity responsible for spinning the software Watchdog. It periodically sets the high and low states of specific GPIO Watchdog pin. Used only with `GPIOControllerPTH12X`. +* `GPIOController`: Class with specific logic for Lynx and Panther robot. +* `Watchdog`: Entity responsible for spinning the software Watchdog. It periodically sets the high and low states of specific GPIO Watchdog pin. Used only with `GPIOController`. ## EStop Implementation of emergency stop handling. * `EStopInterface`: Interface for versioned emergency stop implementations. -* `EStopPTH12X`: Class with specific logic for the Panther robot with version 1.20 and above. -* `EStopPTH10X`: Class with specific logic for the Panther robot with version below 1.20. +* `EStop`: Class with specific logic for the Husarion UGV. -## PantherSystemRosInterface +## SystemRosInterface -A class that takes care of additional ROS interface of panther system, such as publishing driver state and providing service for clearing errors. +A class that takes care of additional ROS interface of Husarion UGV system, such as publishing driver state and providing service for clearing errors. -## PantherSystem +## {Robot}System The main class that implements SystemInterface from ros2_control (for details refer to the [ros2_control documentation](https://control.ros.org/master/index.html)). Handles transitions (initialization, activation, shutdown, error, etc.), provides interfaces for feedback (position, velocity, effort) and commands (velocity). diff --git a/panther_hardware_interfaces/README.md b/husarion_ugv_hardware_interfaces/README.md similarity index 85% rename from panther_hardware_interfaces/README.md rename to husarion_ugv_hardware_interfaces/README.md index 99b79859..980b9367 100644 --- a/panther_hardware_interfaces/README.md +++ b/husarion_ugv_hardware_interfaces/README.md @@ -1,21 +1,21 @@ -# panther_hardware_interfaces +# husarion_ugv_hardware_interfaces -Package that implements SystemInterface from ros2_control for Panther. +Package that implements SystemInterface from ros2_control for Husarion UGV. ## ROS Nodes -This package doesn't contain any standalone nodes, only plugins that are loaded by the resource manager. To use this hardware interface, add it to your URDF. You can check how to do it in [panther_description](https://github.com/husarion/panther_ros/tree/ros2/panther_description/). +This package doesn't contain any standalone nodes, only plugins that are loaded by the resource manager. To use this hardware interface, add it to your URDF. You can check how to do it in [lynx_description](https://github.com/husarion/panther_ros/tree/ros2/panther_description/) or [panther_description](https://github.com/husarion/panther_ros/tree/ros2/panther_description/). -### PantherSystem +### UGVSystem (PantherSystem | LynxSystem) -Plugin responsible for communicating with engine controllers via the CAN bus, providing E-Stop functionalities and managing Built-in Computer GPIO ports. +Plugins for Panther and Lynx are based on an abstraction called UGVSystem. Most parts of both systems are similar and are described below. Each plugin is responsible for communicating with engine controllers via the CAN bus, providing E-Stop functionality, and managing the built-in computer's GPIO ports. #### Publishers -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther system diagnostic messages. +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System diagnostic messages. - `hardware/e_stop` [*std_msgs/Bool*]: Current E-stop state. - `hardware/io_state` [*panther_msgs/IOState*]: Current IO state. -- `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags. +- `hardware/robot_driver_state` [*panther_msgs/RobotDriverState*]: Current motor controllers' state and error flags. #### Service Servers @@ -30,7 +30,7 @@ Plugin responsible for communicating with engine controllers via the CAN bus, pr #### Parameters -Required parameters are defined when including the interface in the URDF (you can check out [panther_macro.urdf.xacro](../panther_description/urdf/panther_macro.urdf.xacro)). +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) or [lynx_macro.urdf.xacro](../lynx_description/urdf/lynx_macro.urdf.xacro)). Physical properties @@ -42,10 +42,8 @@ Physical properties CAN settings -- `can_interface_name` [*string*, default: **panther_can**]: Name of the CAN interface. +- `can_interface_name` [*string*, default: **robot_can**]: Name of the CAN interface. - `master_can_id` [*int*, default: **3**]: CAN ID of the master device (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)). -- `front_driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)). -- `rear_driver_can_id` [*int*, default: **2**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)). - `sdo_operation_timeout_ms` [*int*, default: **100**]: Timeout of the SDO operations, currently no SDO operation is required in RT operation, so this timeout can be set to a higher value. - `pdo_motor_states_timeout_ms` [*int*, default: **15**]: Depends on the frequency at which Roboteq is configured to send motor states (PDO 1 and 2) data. By default, there should be 10 **[ms]** between received data, if it takes more than `pdo_motor_states_timeout_ms`, a motor states read error is triggered. The default value is set to be expected period +50% margin. - `pdo_driver_state_timeout_ms` [*int*, default: **75**]: Depends on the frequency at which Roboteq is configured to send driver state (PDO 3 and 4) data. By default, there should be 50 **[ms]** between received data, if it takes more than `pdo_driver_state_timeout_ms`, a driver state read error is triggered. The default value is set to be expected period +50% margin. @@ -56,10 +54,19 @@ CAN settings - `max_read_pdo_motor_states_errors_count` [*int*, default: **2**]: How many consecutive errors can happen before escalating to general error. - `max_read_pdo_driver_state_errors_count` [*int*, default: **2**]: How many consecutive errors can happen before escalating to general error. +PantherSystem additional CAN settings + +- `front_driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)). +- `rear_driver_can_id` [*int*, default: **2**, **Required only for PantherSystem**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)). + +LynxSystem additional CAN settings + +- `driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)). + > [!CAUTION] > `max_write_pdo_cmds_errors_count`, `max_read_pdo_motor_states_errors_count`, `max_read_pdo_driver_state_errors_count`, `sdo_operation_timeout`, `pdo_motor_states_timeout_ms` and `pdo_driver_state_timeout_ms` are safety-critical parameters, they should be changed only in very specific cases, be sure that you know how they work and be really cautious when changing them. -### PantherImuSensor +### PhidgetImuSensor Plugin responsible for communicating with IMU and filtering data using Madgwick Filter. @@ -141,24 +148,18 @@ To configure RT check out the instructions provided in the [ros2_control docs](h ### Setup -First, it is necessary to set up a virtual CAN: - - +Since some of the tests create and access a virtual CAN interface, it is necessary to install `kmod` and `iproute2`. ```bash -sudo modprobe vcan -sudo ip link add dev panther_can type vcan -sudo ip link set up panther_can -sudo ip link set panther_can down -sudo ip link set panther_can txqueuelen 1000 -sudo ip link set panther_can up +sudo apt update +sudo apt install -y kmod iproute2 ``` ### Running tests ```bash -colcon build --packages-select panther_hardware_interfaces --symlink-install -colcon test --event-handlers console_direct+ --packages-select panther_hardware_interfaces --parallel-workers 1 +colcon build --packages-select husarion_ugv_hardware_interfaces --symlink-install +colcon test --event-handlers console_direct+ --packages-select husarion_ugv_hardware_interfaces --parallel-workers 1 colcon test-result --verbose --all ``` diff --git a/panther_hardware_interfaces/cmake/SuperBuild.cmake b/husarion_ugv_hardware_interfaces/cmake/SuperBuild.cmake similarity index 98% rename from panther_hardware_interfaces/cmake/SuperBuild.cmake rename to husarion_ugv_hardware_interfaces/cmake/SuperBuild.cmake index c7fca72e..685b8d1b 100644 --- a/panther_hardware_interfaces/cmake/SuperBuild.cmake +++ b/husarion_ugv_hardware_interfaces/cmake/SuperBuild.cmake @@ -46,7 +46,7 @@ ExternalProject_Add( BUILD_IN_SOURCE 1) ExternalProject_Add( - ep_panther_hardware_interfaces + ep_husarion_ugv_hardware_interfaces DEPENDS ${DEPENDENCIES} SOURCE_DIR ${PROJECT_SOURCE_DIR} CMAKE_ARGS -DUSE_SUPERBUILD=OFF diff --git a/panther_hardware_interfaces/config/canopen_configuration.yaml b/husarion_ugv_hardware_interfaces/config/canopen_configuration.yaml similarity index 100% rename from panther_hardware_interfaces/config/canopen_configuration.yaml rename to husarion_ugv_hardware_interfaces/config/canopen_configuration.yaml diff --git a/panther_hardware_interfaces/config/master.dcf b/husarion_ugv_hardware_interfaces/config/master.dcf similarity index 100% rename from panther_hardware_interfaces/config/master.dcf rename to husarion_ugv_hardware_interfaces/config/master.dcf diff --git a/panther_hardware_interfaces/config/phidgets_spatial_parameters.yaml b/husarion_ugv_hardware_interfaces/config/phidgets_spatial_parameters.yaml similarity index 100% rename from panther_hardware_interfaces/config/phidgets_spatial_parameters.yaml rename to husarion_ugv_hardware_interfaces/config/phidgets_spatial_parameters.yaml diff --git a/panther_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds b/husarion_ugv_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds similarity index 100% rename from panther_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds rename to husarion_ugv_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds diff --git a/husarion_ugv_hardware_interfaces/husarion_ugv_hardware_interfaces.xml b/husarion_ugv_hardware_interfaces/husarion_ugv_hardware_interfaces.xml new file mode 100644 index 00000000..47bd30d6 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/husarion_ugv_hardware_interfaces.xml @@ -0,0 +1,25 @@ + + + + Hardware system controller for Panther's Roboteq motor controller + + + + + + Hardware system controller for Lynx's Roboteq motor controller + + + + + + Hardware IMU sensor for Phidget Spatial IMU + + + diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp similarity index 91% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp index 5ac14ee4..ea3942f5 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_IMU_SENSOR_PANTHER_IMU_SENSOR_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_IMU_SENSOR_PANTHER_IMU_SENSOR_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_PHIDGET_IMU_SENSOR_PHIDGET_IMU_SENSOR_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_PHIDGET_IMU_SENSOR_PHIDGET_IMU_SENSOR_HPP_ #include #include @@ -43,7 +43,7 @@ using namespace std::placeholders; -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { using return_type = hardware_interface::return_type; @@ -52,12 +52,12 @@ using StateInterface = hardware_interface::StateInterface; using CommandInterface = hardware_interface::CommandInterface; /** - * @brief Class that implements SensorInterface from ros2_control for Panther + * @brief Class that implements SensorInterface from ros2_control for PhidgetSpatial IMU */ -class PantherImuSensor : public hardware_interface::SensorInterface +class PhidgetImuSensor : public hardware_interface::SensorInterface { public: - RCLCPP_SHARED_PTR_DEFINITIONS(PantherImuSensor) + RCLCPP_SHARED_PTR_DEFINITIONS(PhidgetImuSensor) CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) override; CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; @@ -143,7 +143,7 @@ class PantherImuSensor : public hardware_interface::SensorInterface std::vector imu_sensor_state_; - rclcpp::Logger logger_{rclcpp::get_logger("PantherImuSensor")}; + rclcpp::Logger logger_{rclcpp::get_logger("PhidgetImuSensor")}; rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; inline static const std::array kImuInterfacesNames = { @@ -180,6 +180,6 @@ class PantherImuSensor : public hardware_interface::SensorInterface rclcpp::Time last_spatial_data_timestamp_; }; -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_IMU_SENSOR_PANTHER_IMU_SENSOR_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_PHIDGET_IMU_SENSOR_PHIDGET_IMU_SENSOR_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp similarity index 55% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp index 993395a6..d147de83 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp @@ -17,8 +17,8 @@ * @brief Header file containing a higher-level wrapper for the GPIO driver. */ -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_ #include #include @@ -29,9 +29,12 @@ #include "gpiod.hpp" -#include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp" +#include "husarion_ugv_utils/common_utilities.hpp" -namespace panther_hardware_interfaces +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp" + +namespace husarion_ugv_hardware_interfaces { class EStopResetInterrupted : public std::exception @@ -54,7 +57,7 @@ class Watchdog * @exception std::runtime_error if the Watchdog pin is not configured by GPIODriver or not * described in GPIOController gpio_info storage */ - Watchdog(std::shared_ptr gpio_driver); + Watchdog(std::shared_ptr gpio_driver); /** * @brief Destructor for Watchdog class. Turns off the watchdog thread. @@ -84,7 +87,7 @@ class Watchdog void WatchdogThread(); GPIOPin watchdog_pin_ = GPIOPin::WATCHDOG; - std::shared_ptr gpio_driver_; + std::shared_ptr gpio_driver_; std::thread watchdog_thread_; std::atomic_bool watchdog_thread_enabled_ = false; }; @@ -127,7 +130,7 @@ class GPIOControllerInterface * }; * * MyClass my_obj; - * GPIOControllerPTH12X gpio_controller; + * GPIOController gpio_controller; * gpio_controller.RegisterGPIOEventCallback( * std::bind(&MyClass::HandleGPIOEvent, &my_obj, std::placeholders::_1)); * @endcode @@ -139,12 +142,20 @@ class GPIOControllerInterface bool IsPinAvailable(const GPIOPin pin) const; protected: - std::shared_ptr gpio_driver_; + std::shared_ptr gpio_driver_; }; -class GPIOControllerPTH12X : public GPIOControllerInterface +class GPIOController : public GPIOControllerInterface { public: + /** + * @brief Constructor for GPIOController class. + * + * @param gpio_driver Pointer to the GPIODriver object. + * @throw `std::runtime_error` When the GPIO driver is not initialized. + */ + GPIOController(std::shared_ptr gpio_driver); + /** * @brief Initializes the GPIODriver, Watchdog, and powers on the motors. */ @@ -162,17 +173,19 @@ class GPIOControllerPTH12X : public GPIOControllerInterface * @brief Resets the E-stop. * * This method verifies the status of the E_STOP_RESET pin, which is configured as an input. - * If the pin is active, it attempts to reset the E-stop by momentarily setting it to an inactive - * state. During this reset process, the pin is configured as an output for a specific duration. - * If the attempt to reset the E-stop fails (the pin reads its value as an input again), it throws - * a runtime error. The Watchdog thread is temporarily activated during the E-stop reset process. + * If the pin is active, it attempts to reset the E-stop by momentarily setting it to an + * inactive state. During this reset process, the pin is configured as an output for a specific + * duration. If the attempt to reset the E-stop fails (the pin reads its value as an input + * again), it throws a runtime error. The Watchdog thread is temporarily activated during the + * E-stop reset process. * @return true if the E-stop is successfully reset. * @exception std::runtime_error when the E-stop reset fails. */ void EStopReset() override; /** - * @brief Controls the motor power by enabling or disabling them based on the 'enable' parameter. + * @brief Controls the motor power by enabling or disabling them based on the 'enable' + * parameter. * * @param enable Set to 'true' to enable the motors, 'false' to disable. * @return 'true' if the motor control pin value is successfully set, 'false' otherwise. @@ -204,7 +217,8 @@ class GPIOControllerPTH12X : public GPIOControllerInterface bool DigitalPowerEnable(const bool enable) override; /** - * @brief Enables or disables the use of an external charger according to the 'enable' parameter. + * @brief Enables or disables the use of an external charger according to the 'enable' + * parameter. * * @param enable Set to 'true' to enable external charger, 'false' to disable. * @return 'true' if the charger control pin value is successfully set, 'false' otherwise. @@ -226,6 +240,11 @@ class GPIOControllerPTH12X : public GPIOControllerInterface */ std::unordered_map QueryControlInterfaceIOStates() const override; + /** + * @brief Returns the GPIO pin configuration information for the PTH12X. + */ + static const std::vector & GetGPIOConfigInfoStorage(); + void InterruptEStopReset() override; protected: @@ -235,9 +254,9 @@ class GPIOControllerPTH12X : public GPIOControllerInterface /** * @brief Waits for a specific duration or until an interruption is signaled. * - * This method is designed to block execution for the specified timeout duration. It also monitors - * for an interruption signal which, if received, will cause the method to return early. The - * interruption is controlled by the `should_abort_e_stop_reset_` flag. + * This method is designed to block execution for the specified timeout duration. It also + * monitors for an interruption signal which, if received, will cause the method to return + * early. The interruption is controlled by the `should_abort_e_stop_reset_` flag. * * @param timeout Duration to wait for in milliseconds. * @return `true` if the wait completed without interruption, `false` if an interruption was @@ -246,135 +265,36 @@ class GPIOControllerPTH12X : public GPIOControllerInterface bool WaitFor(std::chrono::milliseconds timeout); /** - * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc. + * @brief Vector containing GPIO pin configuration information such as pin direction, value, + * etc. */ - const std::vector gpio_config_info_storage_{ - GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::GPIN1, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::GPIN2, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT, true}, - GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true}, - }; + static const std::vector gpio_config_info_storage_; std::mutex e_stop_cv_mtx_; std::condition_variable e_stop_cv_; volatile std::atomic_bool should_abort_e_stop_reset_ = false; }; -class GPIOControllerPTH10X : public GPIOControllerInterface +class GPIOControllerFactory { public: /** - * @brief Initializes the GPIODriver and powers on the motors. - */ - void Start() override; - - /** - * @brief Placeholder method indicating lack of hardware E-stop support for the robot in this - * version. - * - * @return Always returns true. - */ - void EStopTrigger() override; - - /** - * @brief Checks if the motors are powered up (when STAGE2_INPUT is active/main switch is set to - * STAGE2 position) without controlling any GPIO. - * - * @exception std::runtime_error when the motors are not powered up. - * @return Always returns true when the motors are powered up. - */ - void EStopReset() override; - - /** - * @brief Controls the motor power by enabling or disabling them based on the 'enable' parameter. - * - * This method checks if the motors are powered up by verifying the 'STAGE2_INPUT' pin. - * - * @param enable Set to 'true' to enable the motors, 'false' to disable. - * @exception std::runtime_error When attempting to enable the motors without the 'STAGE2_INPUT' - * pin active. - * @return 'true' if the motor control pin value is successfully set, 'false' otherwise. - */ - bool MotorPowerEnable(const bool enable) override; - - /** - * @brief Placeholder method indicating lack of support for controlling the fan in this robot - * version. - * - * @param enable Ignored parameter in this version. - * @exception std::runtime_error Always throws a runtime error due to lack of support for fan - * control. - */ - bool FanEnable(const bool /* enable */) override; - - /** - * @brief Placeholder method indicating lack of support for controlling AUX in this robot - * version. - * - * @param enable Ignored parameter in this version. - * @exception std::runtime_error Always throws a runtime error due to lack of support for AUX - * power control. - */ - bool AUXPowerEnable(const bool /* enable */) override; - - /** - * @brief Placeholder method indicating lack of support for controlling Digital Power in this - * robot version. - * - * @param enable Ignored parameter in this version. - * @exception std::runtime_error Always throws a runtime error due to lack of support for digital - * power control. - */ - bool DigitalPowerEnable(const bool /* enable */) override; - - /** - * @brief Placeholder method indicating lack of support for enabling external charger in this - * robot version. - * - * @param enable Ignored parameter in this version. - * @exception std::runtime_error Always throws a runtime error due to lack of support for charging - * process control. - */ - bool ChargerEnable(const bool /* enable */) override; - - /** - * @brief Enables or disables the LED control based on the 'enable' parameter. + * @brief Creates a GPIO controller. * - * @param enable Set to 'true' to enable the LED control, 'false' to disable. - * @return 'true' if the LED control pin value is successfully set, 'false' otherwise. + * @return A unique pointer to the created GPIO controller. */ - bool LEDControlEnable(const bool enable) override; + static std::unique_ptr CreateGPIOController() + { + std::unique_ptr gpio_controller; + auto config_info_storage = GPIOController::GetGPIOConfigInfoStorage(); + auto gpio_driver = std::make_shared(config_info_storage); - /** - * @brief Returns imitation of the IO states of the control interface. In this version of the - * robot, there is a lack of support for controlling these IOs. - * - * @return An unordered map containing the GPIOPin as the key and its active state as the value. - */ - std::unordered_map QueryControlInterfaceIOStates() const override; + gpio_controller = std::make_unique(gpio_driver); -private: - /** - * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc. - */ - const std::vector gpio_config_info_storage_{ - GPIOInfo{GPIOPin::STAGE2_INPUT, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true}, + return gpio_controller; }; }; -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_CONTROLLER_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp similarity index 80% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp index 9c45fa28..67c8f790 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_DRIVER_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_DRIVER_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_DRIVER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_DRIVER_HPP_ #include #include @@ -29,51 +29,32 @@ #include "gpiod.hpp" -namespace panther_hardware_interfaces -{ +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp" -/** - * @brief Enumeration representing available GPIO pins in the Panther system. - */ -enum class GPIOPin { - AUX_PW_EN, - CHRG_DISABLE, - CHRG_SENSE, - DRIVER_EN, - E_STOP_RESET, - FAN_SW, - GPOUT1, - GPOUT2, - GPIN1, - GPIN2, - LED_SBC_SEL, - SHDN_INIT, - STAGE2_INPUT, - VDIG_OFF, - VMOT_ON, - MOTOR_ON, - WATCHDOG -}; +namespace husarion_ugv_hardware_interfaces +{ -/** - * @brief Structure containing information related to GPIO pins such as pin configuration, - * direction, value, etc. This information is required during the initialization process. - */ -struct GPIOInfo +class GPIODriverInterface { - GPIOPin pin; - gpiod::line::direction direction; - bool active_low = false; - gpiod::line::value init_value = gpiod::line::value::INACTIVE; - gpiod::line::value value = gpiod::line::value::INACTIVE; - gpiod::line::offset offset = -1; +public: + virtual ~GPIODriverInterface() = default; + virtual void GPIOMonitorEnable( + const bool use_rt = false, const unsigned gpio_monit_thread_sched_priority = 60) = 0; + virtual void ConfigureEdgeEventCallback( + const std::function & callback) = 0; + virtual void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction) = 0; + virtual bool IsPinAvailable(const GPIOPin pin) const = 0; + virtual bool IsPinActive(const GPIOPin pin) = 0; + virtual bool SetPinValue( + const GPIOPin pin, const bool value, + const std::chrono::milliseconds & pin_validation_wait_time = std::chrono::milliseconds(0)) = 0; }; /** * @brief Class responsible for managing GPIO pins on Panther robots, handling tasks such as * setting pin values, changing pin directions, monitoring pin events, and more. */ -class GPIODriver +class GPIODriver : public GPIODriverInterface { public: /** @@ -127,7 +108,7 @@ class GPIODriver * lack of functionality to read pin values. */ void GPIOMonitorEnable( - const bool use_rt = false, const unsigned gpio_monit_thread_sched_priority = 60); + const bool use_rt = false, const unsigned gpio_monit_thread_sched_priority = 60) override; /** * @brief This method sets the provided callback function to be executed upon GPIO edge events. @@ -154,7 +135,7 @@ class GPIODriver * std::bind(&MyClass::HandleGPIOEvent, &my_obj, std::placeholders::_1)); * @endcode */ - void ConfigureEdgeEventCallback(const std::function & callback); + void ConfigureEdgeEventCallback(const std::function & callback) override; /** * @brief Changes the direction of a specific GPIO pin. @@ -162,7 +143,7 @@ class GPIODriver * @param pin GPIOPin to change the direction for. * @param direction New direction for the pin. */ - void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction); + void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction) override; /** * @brief Returns true if a specific pin is configured and stored in GPIO info storage @@ -170,7 +151,7 @@ class GPIODriver * @param pin The GPIO pin to check availability for * @return true if the pin is available, false otherwise */ - bool IsPinAvailable(const GPIOPin pin) const; + bool IsPinAvailable(const GPIOPin pin) const override; /** * @brief Checks if a specific GPIO pin is active. This method returns the value stored in the @@ -182,7 +163,7 @@ class GPIODriver * * @return True if the pin is active, false otherwise. */ - bool IsPinActive(const GPIOPin pin); + bool IsPinActive(const GPIOPin pin) override; /** * @brief Sets the value for a specific GPIO pin. @@ -199,7 +180,8 @@ class GPIODriver */ bool SetPinValue( const GPIOPin pin, const bool value, - const std::chrono::milliseconds & pin_validation_wait_time = std::chrono::milliseconds(0)); + const std::chrono::milliseconds & pin_validation_wait_time = + std::chrono::milliseconds(0)) override; private: std::unique_ptr CreateLineRequest(gpiod::chip & chip); @@ -232,29 +214,6 @@ class GPIODriver */ std::function GPIOEdgeEventCallback; - /** - * @brief Mapping of GPIO pins to their respective names. - */ - const std::map pin_names_{ - {GPIOPin::WATCHDOG, "WATCHDOG"}, - {GPIOPin::AUX_PW_EN, "AUX_PW_EN"}, - {GPIOPin::CHRG_DISABLE, "CHRG_DISABLE"}, - {GPIOPin::CHRG_SENSE, "CHRG_SENSE"}, - {GPIOPin::DRIVER_EN, "DRIVER_EN"}, - {GPIOPin::E_STOP_RESET, "E_STOP_RESET"}, - {GPIOPin::FAN_SW, "FAN_SW"}, - {GPIOPin::GPOUT1, "GPOUT1"}, - {GPIOPin::GPOUT2, "GPOUT2"}, - {GPIOPin::GPIN1, "GPIN1"}, - {GPIOPin::GPIN2, "GPIN2"}, - {GPIOPin::LED_SBC_SEL, "LED_SBC_SEL"}, - {GPIOPin::SHDN_INIT, "SHDN_INIT"}, - {GPIOPin::STAGE2_INPUT, "STAGE2_INPUT"}, - {GPIOPin::VDIG_OFF, "VDIG_OFF"}, - {GPIOPin::VMOT_ON, "VMOT_ON"}, - {GPIOPin::MOTOR_ON, "MOTOR_ON"}, - }; - /** * @brief Vector containing GPIO pin configuration information such as pin direction, value, etc. */ @@ -295,6 +254,6 @@ class GPIODriver const std::filesystem::path gpio_chip_path_ = "/dev/gpiochip0"; }; -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_GPIO_GPIO_DRIVER_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_GPIO_DRIVER_HPP_ diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp new file mode 100644 index 00000000..987380ba --- /dev/null +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp @@ -0,0 +1,88 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_TYPES_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_TYPES_HPP_ + +#include +#include + +#include "gpiod.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +/** + * @brief Enumeration representing available GPIO pins in the Panther system. + */ +enum class GPIOPin { + AUX_PW_EN, + CHRG_DISABLE, + CHRG_SENSE, + DRIVER_EN, + E_STOP_RESET, + FAN_SW, + GPOUT1, + GPOUT2, + GPIN1, + GPIN2, + LED_SBC_SEL, + SHDN_INIT, + STAGE2_INPUT, + VDIG_OFF, + VMOT_ON, + MOTOR_ON, + WATCHDOG +}; + +/** + * @brief Mapping of GPIO pins to their respective names. + */ +const std::map pin_names_{ + {GPIOPin::WATCHDOG, "WATCHDOG"}, + {GPIOPin::AUX_PW_EN, "AUX_PW_EN"}, + {GPIOPin::CHRG_DISABLE, "CHRG_DISABLE"}, + {GPIOPin::CHRG_SENSE, "CHRG_SENSE"}, + {GPIOPin::DRIVER_EN, "DRIVER_EN"}, + {GPIOPin::E_STOP_RESET, "E_STOP_RESET"}, + {GPIOPin::FAN_SW, "FAN_SW"}, + {GPIOPin::GPOUT1, "GPOUT1"}, + {GPIOPin::GPOUT2, "GPOUT2"}, + {GPIOPin::GPIN1, "GPIN1"}, + {GPIOPin::GPIN2, "GPIN2"}, + {GPIOPin::LED_SBC_SEL, "LED_SBC_SEL"}, + {GPIOPin::SHDN_INIT, "SHDN_INIT"}, + {GPIOPin::STAGE2_INPUT, "STAGE2_INPUT"}, + {GPIOPin::VDIG_OFF, "VDIG_OFF"}, + {GPIOPin::VMOT_ON, "VMOT_ON"}, + {GPIOPin::MOTOR_ON, "MOTOR_ON"}, +}; + +/** + * @brief Structure containing information related to GPIO pins such as pin configuration, + * direction, value, etc. This information is required during the initialization process. + */ +struct GPIOInfo +{ + GPIOPin pin; + gpiod::line::direction direction; + bool active_low = false; + gpiod::line::value init_value = gpiod::line::value::INACTIVE; + gpiod::line::value value = gpiod::line::value::INACTIVE; + gpiod::line::offset offset = -1; +}; + +} // namespace husarion_ugv_hardware_interfaces + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_GPIO_TYPES_HPP_ diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp new file mode 100644 index 00000000..40a61823 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp @@ -0,0 +1,60 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_LYNX_SYSTEM_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_LYNX_SYSTEM_HPP_ + +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +/** + * @brief Class that implements UGVSystem for Lynx robot + */ +class LynxSystem : public UGVSystem +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(LynxSystem) + + LynxSystem() : UGVSystem(kJointOrder) {} + + ~LynxSystem() = default; + +protected: + void ReadCANopenSettingsDriverCANIDs() override; + + virtual void DefineRobotDriver() override; // virtual for testing + + void UpdateHwStates() override; + void UpdateMotorsStateDataTimedOut() override; + + void UpdateDriverStateMsg() override; + void UpdateFlagErrors() override; + void UpdateDriverStateDataTimedOut() override; + + std::vector GetSpeedCommands() const; + + void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) override; + void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) override; + + static const inline std::vector kJointOrder = {"fl", "fr", "rl", "rr"}; +}; + +} // namespace husarion_ugv_hardware_interfaces + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_LYNX_SYSTEM_HPP_ diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp new file mode 100644 index 00000000..acd1217e --- /dev/null +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp @@ -0,0 +1,60 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_PANTHER_SYSTEM_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_PANTHER_SYSTEM_HPP_ + +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +/** + * @brief Class that implements UGVSystem for Panther robot + */ +class PantherSystem : public UGVSystem +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(PantherSystem) + + PantherSystem() : UGVSystem(kJointOrder) {} + + ~PantherSystem() = default; + +protected: + void ReadCANopenSettingsDriverCANIDs() override; + + virtual void DefineRobotDriver() override; // virtual for testing + + void UpdateHwStates() override; + void UpdateMotorsStateDataTimedOut() override; + + void UpdateDriverStateMsg() override; + void UpdateFlagErrors() override; + void UpdateDriverStateDataTimedOut() override; + + std::vector GetSpeedCommands() const; + + void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) override; + void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) override; + + static const inline std::vector kJointOrder = {"fl", "fr", "rl", "rr"}; +}; + +} // namespace husarion_ugv_hardware_interfaces + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_PANTHER_SYSTEM_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp similarity index 67% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp index 883d7363..8ca1fbe0 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_ #include #include @@ -33,18 +33,21 @@ #include "lely/io2/sys/sigset.hpp" #include "lely/io2/sys/timer.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp" +namespace husarion_ugv_hardware_interfaces +{ -namespace panther_hardware_interfaces +struct CANopenObject { + const std::uint16_t id; + const std::uint8_t subid; +}; struct CANopenSettings { std::string can_interface_name; std::uint8_t master_can_id; - std::uint8_t front_driver_can_id; - std::uint8_t rear_driver_can_id; + std::map driver_can_ids; std::chrono::milliseconds pdo_motor_states_timeout_ms; std::chrono::milliseconds pdo_driver_state_timeout_ms; @@ -52,15 +55,15 @@ struct CANopenSettings }; /** - * @brief CANopenController takes care of CANopen communication - creates master controller + * @brief CANopenManager takes care of CANopen communication - creates master controller * and two Roboteq drivers (front and rear) */ -class CANopenController +class CANopenManager { public: - CANopenController(const CANopenSettings & canopen_settings); + CANopenManager(const CANopenSettings & canopen_settings); - ~CANopenController() { Deinitialize(); } + ~CANopenManager() { Deinitialize(); } /** * @brief Starts CANopen communication (in a new thread) and waits for boot to finish @@ -74,8 +77,27 @@ class CANopenController */ void Deinitialize(); - std::shared_ptr GetFrontDriver() { return front_driver_; } - std::shared_ptr GetRearDriver() { return rear_driver_; } + /** + * @brief Activates CANopen communication thread. This method should be invoked after all objects + * using this communication are created. + * + * @exception std::runtime_error if CAN communication not activated or not initialized + */ + void Activate(); + + /** + * @brief Returns master controller + * + * @return std::shared_ptr master controller + * @exception std::runtime_error if CANopenManager is not initialized + */ + std::shared_ptr GetMaster() + { + if (!initialized_) { + throw std::runtime_error("CANopenManager not initialized."); + } + return master_; + } private: void InitializeCANCommunication(); @@ -88,13 +110,6 @@ class CANopenController */ void NotifyCANCommunicationStarted(const bool result); - /** - * @brief Triggers boot on front and rear Roboteq drivers and waits for finish - * - * @exception std::runtime_error if boot fails - */ - void BootDrivers(); - // Priority set to be higher than the priority of the main ros2 control node (50) static constexpr unsigned kCANopenThreadSchedPriority = 60; @@ -115,12 +130,9 @@ class CANopenController std::shared_ptr chan_; std::shared_ptr master_; - std::shared_ptr front_driver_; - std::shared_ptr rear_driver_; - const CANopenSettings canopen_settings_; }; -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_CANOPEN_CONTROLLER_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_CANOPEN_MANAGER_HPP_ diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp new file mode 100644 index 00000000..760cfde5 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp @@ -0,0 +1,150 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "lely/coapp/loop_driver.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +struct MotorDriverState +{ + std::int32_t pos; + std::int16_t vel; + std::int16_t current; + + timespec pos_timestamp; + timespec vel_current_timestamp; +}; + +struct DriverState +{ + std::uint8_t fault_flags; + std::uint8_t script_flags; + std::uint8_t runtime_stat_flag_channel_1; + std::uint8_t runtime_stat_flag_channel_2; + + std::int16_t battery_current_1; + std::int16_t battery_current_2; + + std::uint16_t battery_voltage; + + std::int16_t mcu_temp; + std::int16_t heatsink_temp; + + timespec flags_current_timestamp; + timespec voltages_temps_timestamp; +}; + +/** + * @brief Abstract class for motor driver + */ +class MotorDriverInterface +{ +public: + /** + * @brief Reads motors' state data returned from (PDO 1 and 2) and saves + * last timestamps + */ + virtual MotorDriverState ReadState() = 0; + + /** + * @brief Sends commands to the motors + * + * @param cmd command value in the range [-1000, 1000] + * + * @exception std::runtime_error if operation fails + */ + virtual void SendCmdVel(const std::int32_t cmd) = 0; + + /** + * @brief Sends a safety stop command to the motor + * + * @exception std::runtime_error if any operation returns error + */ + virtual void TurnOnSafetyStop() = 0; +}; + +/** + * @brief Abstract class that provides functionality for managing motor drivers + */ +class DriverInterface +{ +public: + /** + * @brief Triggers boot operations + * + * @exception std::runtime_error if triggering boot fails + */ + virtual std::future Boot() = 0; + + /** + * @brief Returns true if CAN error was detected. + */ + virtual bool IsCANError() const = 0; + + /** + * @brief Returns true if heartbeat timeout encountered. + */ + virtual bool IsHeartbeatTimeout() const = 0; + + /** + * @brief Reads driver state data returned from (PDO 3 and 4): error flags, battery + * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents), + * temperatures. Also saves the last timestamps + */ + virtual DriverState ReadState() = 0; + + /** + * @exception std::runtime_error if any operation returns error + */ + virtual void ResetScript() = 0; + + /** + * @exception std::runtime_error if any operation returns error + */ + virtual void TurnOnEStop() = 0; + + /** + * @exception std::runtime_error if any operation returns error + */ + virtual void TurnOffEStop() = 0; + + /** + * @brief Adds a motor driver to the driver + */ + virtual void AddMotorDriver( + const std::string name, std::shared_ptr motor_driver) = 0; + + virtual std::shared_ptr GetMotorDriver(const std::string & name) = 0; + + /** + * @brief Alias for a shared pointer to a Driver object. + */ + using SharedPtr = std::shared_ptr; +}; + +} // namespace husarion_ugv_hardware_interfaces + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_DRIVER_HPP_ diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp new file mode 100644 index 00000000..4991373a --- /dev/null +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp @@ -0,0 +1,71 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_ + +#include +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +/** + * @brief This class implements RoboteqRobotDriver for Lynx robot. + * It defines one Roboteq controller with two motors controlling left and right wheels. + */ +class LynxRobotDriver : public RoboteqRobotDriver +{ +public: + LynxRobotDriver( + const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings, + const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000)) + : RoboteqRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time) + { + } + + ~LynxRobotDriver() = default; + + /** + * @brief Write speed commands to motors + * + * @param speeds vector of motor speeds in rad/s. The order is: left, right + * + * @exception std::runtime_error if invalid vector size, send command fails or CAN error is + * detected + */ + void SendSpeedCommands(const std::vector & speeds) override; + + /** + * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq + * driver faults still exist, the error flag will remain active. + */ + void AttemptErrorFlagReset() override { SendSpeedCommands({0.0, 0.0}); }; + +protected: + /** + * @brief This method defines driver objects and adds motor drivers for them. It is virtual to + * allow mocking drivers in tests. + */ + void DefineDrivers() override; +}; + +} // namespace husarion_ugv_hardware_interfaces + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_LYNX_ROBOT_DRIVER_HPP_ diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp new file mode 100644 index 00000000..d72d36f2 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp @@ -0,0 +1,73 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_ + +#include +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +/** + * @brief This class implements the RoboteqRobotDriver for the Panther robot. + * It defines two Roboteq drivers (front and rear) with two motors each for controlling the left + * and right wheels. + */ +class PantherRobotDriver : public RoboteqRobotDriver +{ +public: + PantherRobotDriver( + const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings, + const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000)) + : RoboteqRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time) + { + } + + ~PantherRobotDriver() = default; + + /** + * @brief Write speed commands to motors + * + * @param speeds vector of motor speeds in rad/s. + * The order is: front left, front right, rear left, rear right + * + * @exception std::runtime_error if invalid vector size, send command fails or CAN error is + * detected + */ + void SendSpeedCommands(const std::vector & speeds) override; + + /** + * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq + * driver faults still exist, the error flag will remain active. + */ + void AttemptErrorFlagReset() override { SendSpeedCommands({0.0, 0.0, 0.0, 0.0}); }; + +protected: + /** + * @brief This method defines driver objects and adds motor drivers for them. It is virtual to + * allow mocking drivers in tests. + */ + virtual void DefineDrivers(); +}; + +} // namespace husarion_ugv_hardware_interfaces + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_PANTHER_ROBOT_DRIVER_HPP_ diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp new file mode 100644 index 00000000..177e6142 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp @@ -0,0 +1,121 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_ + +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +/** + * @brief Abstract class for managing different robot drivers. + */ +class RobotDriverInterface +{ +public: + /** + * @brief Initialize robot driver + * + * @exception std::runtime_error if boot fails + */ + virtual void Initialize() = 0; + + /** + * @brief Deinitialize robot driver + */ + virtual void Deinitialize() = 0; + + /** + * @brief Activate procedure for the driver + * + * @exception std::runtime_error if any procedure step fails + */ + virtual void Activate() = 0; + + /** + * @brief Updates current communication state with the drivers + * + * @exception std::runtime_error if error was detected + */ + virtual void UpdateCommunicationState() = 0; + + /** + * @brief Updates current motors' state (position, velocity, current). + * + * @exception std::runtime_error if error was detected + */ + virtual void UpdateMotorsState() = 0; + + /** + * @brief Updates current driver state (flags, temperatures, voltage, battery current) + * + * @exception std::runtime_error if error was detected + */ + virtual void UpdateDriversState() = 0; + + /** + * @brief Get data feedback from the driver + * + * @param name name of the data to get + * + * @return data feedback + * @exception std::runtime_error if data with the given name does not exist + */ + virtual const DriverData & GetData(const std::string & name) = 0; + + /** + * @brief Write speed commands to motors + * + * @param speeds vector of motor speeds in rad/s + * + * @exception std::runtime_error if vector has invalid size or send command fails + */ + virtual void SendSpeedCommands(const std::vector & speeds) = 0; + + /** + * @brief Turns on E-stop + * + * @exception std::runtime_error if any operation returns error + */ + virtual void TurnOnEStop() = 0; + + /** + * @brief Turns off E-stop + * + * @exception std::runtime_error if any operation returns error + */ + virtual void TurnOffEStop() = 0; + + /** + * @brief Attempt to clear driver error flags. If driver faults still exist, + * the error flags should remain active. + */ + virtual void AttemptErrorFlagReset() = 0; + + /** + * @brief Check if communication with drivers is working properly + * + * @return true if communication error occurred + */ + virtual bool CommunicationError() = 0; +}; + +} // namespace husarion_ugv_hardware_interfaces + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOT_DRIVER_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp similarity index 74% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp index 0a167e7c..ba3649db 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DATA_CONVERTERS_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DATA_CONVERTERS_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_ #include #include @@ -25,10 +25,10 @@ #include "panther_msgs/msg/runtime_error.hpp" #include "panther_msgs/msg/script_flag.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp" -#include "panther_hardware_interfaces/utils.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp" +#include "husarion_ugv_hardware_interfaces/utils.hpp" -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { struct DrivetrainSettings @@ -71,7 +71,7 @@ class MotorState public: MotorState(const DrivetrainSettings & drivetrain_settings); - void SetData(const RoboteqMotorState & motor_state) { motor_state_ = motor_state; }; + void SetData(const MotorDriverState & motor_state) { motor_state_ = motor_state; }; float GetPosition() const { return motor_state_.pos * roboteq_pos_feedback_to_radians_; } float GetVelocity() const @@ -88,7 +88,7 @@ class MotorState float roboteq_vel_feedback_to_radians_per_second_; float roboteq_current_feedback_to_newton_meters_; - RoboteqMotorState motor_state_ = {0, 0, 0}; + MotorDriverState motor_state_ = {0, 0, 0, {0, 0}, {0, 0}}; }; /** @@ -148,10 +148,10 @@ class RuntimeError : public FlagError * @brief Class for storing and converting the current state of the Roboteq drivers (temperature, * voltage and battery current) */ -class DriverState +class RoboteqDriverState { public: - DriverState() {} + RoboteqDriverState() {} void SetTemperature(const std::int16_t temp) { temp_ = temp; }; void SetHeatsinkTemperature(const std::int16_t heatsink_temp) { heatsink_temp_ = heatsink_temp; }; @@ -181,25 +181,25 @@ class DriverState /** * @brief Class that combines all the data that the one Roboteq driver provides */ -class RoboteqData +class DriverData { public: - RoboteqData(const DrivetrainSettings & drivetrain_settings) - : left_motor_state_(drivetrain_settings), right_motor_state_(drivetrain_settings) + DriverData(const DrivetrainSettings & drivetrain_settings) + : channel_1_motor_state_(drivetrain_settings), channel_2_motor_state_(drivetrain_settings) { } void SetMotorsStates( - const RoboteqMotorState & left_state, const RoboteqMotorState & right_state, + const MotorDriverState & channel_1_state, const MotorDriverState & channel_2_state, const bool data_timed_out); - void SetDriverState(const RoboteqDriverState & state, const bool data_timed_out); + void SetDriverState(const DriverState & state, const bool data_timed_out); void SetCANError(const bool can_error) { can_error_ = can_error; } void SetHeartbeatTimeout(const bool heartbeat_timeout) { heartbeat_timeout_ = heartbeat_timeout; } bool IsFlagError() const { - return fault_flags_.IsError() || script_flags_.IsError() || left_runtime_error_.IsError() || - right_runtime_error_.IsError(); + return fault_flags_.IsError() || script_flags_.IsError() || + channel_1_runtime_error_.IsError() || channel_2_runtime_error_.IsError(); } bool IsError() const @@ -208,9 +208,16 @@ class RoboteqData can_error_ || heartbeat_timeout_; } - const MotorState & GetLeftMotorState() const { return left_motor_state_; } - const MotorState & GetRightMotorState() const { return right_motor_state_; } - const DriverState & GetDriverState() const { return driver_state_; } + /** + * @brief Returns motor state data for the given channel + * + * @param channel 1 or 2 + * @return motor state data + * @throws std::runtime_error if invalid channel number + */ + const MotorState & GetMotorState(const std::uint8_t channel) const; + + const RoboteqDriverState & GetDriverState() const { return driver_state_; } bool IsMotorStatesDataTimedOut() const { return motor_states_data_timed_out_; } bool IsDriverStateDataTimedOut() const { return driver_state_data_timed_out_; } @@ -219,8 +226,15 @@ class RoboteqData const FaultFlag & GetFaultFlag() const { return fault_flags_; } const ScriptFlag & GetScriptFlag() const { return script_flags_; } - const RuntimeError & GetLeftRuntimeError() const { return left_runtime_error_; } - const RuntimeError & GetRightRuntimeError() const { return right_runtime_error_; } + + /** + * @brief Returns runtime error flags for the given channel + * + * @param channel 1 or 2 + * @return runtime error flags + * @throws std::runtime_error if invalid channel number + */ + const RuntimeError & GetRuntimeError(const std::uint8_t channel) const; std::string GetFlagErrorLog() const; @@ -228,15 +242,15 @@ class RoboteqData std::map GetErrorMap() const; private: - MotorState left_motor_state_; - MotorState right_motor_state_; + MotorState channel_1_motor_state_; + MotorState channel_2_motor_state_; - DriverState driver_state_; + RoboteqDriverState driver_state_; FaultFlag fault_flags_; ScriptFlag script_flags_; - RuntimeError left_runtime_error_; - RuntimeError right_runtime_error_; + RuntimeError channel_1_runtime_error_; + RuntimeError channel_2_runtime_error_; bool motor_states_data_timed_out_ = false; bool driver_state_data_timed_out_ = false; @@ -244,6 +258,6 @@ class RoboteqData bool heartbeat_timeout_ = false; }; -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DATA_CONVERTERS_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DATA_CONVERTERS_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp similarity index 53% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp index 498231c2..729339ab 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DRIVER_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DRIVER_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_ #include #include @@ -25,48 +25,19 @@ #include "lely/coapp/loop_driver.hpp" -namespace panther_hardware_interfaces -{ +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp" -struct RoboteqMotorState +namespace husarion_ugv_hardware_interfaces { - std::int32_t pos; - std::int16_t vel; - std::int16_t current; -}; - -struct RoboteqMotorsStates -{ - RoboteqMotorState motor_1; - RoboteqMotorState motor_2; - - timespec pos_timestamp; - timespec vel_current_timestamp; -}; - -struct RoboteqDriverState -{ - std::uint8_t fault_flags; - std::uint8_t script_flags; - std::uint8_t runtime_stat_flag_motor_1; - std::uint8_t runtime_stat_flag_motor_2; - - std::int16_t battery_current_1; - std::int16_t battery_current_2; - std::uint16_t battery_voltage; - - std::int16_t mcu_temp; - std::int16_t heatsink_temp; - - timespec flags_current_timestamp; - timespec voltages_temps_timestamp; -}; +// Forward declaration +class RoboteqMotorDriver; /** - * @brief Implementation of LoopDriver for Roboteq drivers + * @brief Hardware implementation of Driver with lely LoopDriver for Roboteq drivers + * control */ -class RoboteqDriver : public lely::canopen::LoopDriver +class RoboteqDriver : public DriverInterface, public lely::canopen::LoopDriver { public: RoboteqDriver( @@ -78,70 +49,53 @@ class RoboteqDriver : public lely::canopen::LoopDriver * * @exception std::runtime_error if triggering boot fails */ - std::future Boot(); + std::future Boot() override; /** * @brief Returns true if CAN error was detected. */ - bool IsCANError() const { return can_error_.load(); } + bool IsCANError() const override { return can_error_.load(); }; /** * @brief Returns true if heartbeat timeout encountered. */ - bool IsHeartbeatTimeout() const { return heartbeat_timeout_.load(); } - - /** - * @brief Reads motors' state data returned from Roboteq (PDO 1 and 2) and saves - * last timestamps - */ - RoboteqMotorsStates ReadRoboteqMotorsStates(); + bool IsHeartbeatTimeout() const override { return heartbeat_timeout_.load(); }; /** * @brief Reads driver state data returned from Roboteq (PDO 3 and 4): error flags, battery * voltage, battery currents (for channel 1 and 2, they are not the same as motor currents), * temperatures. Also saves the last timestamps */ - RoboteqDriverState ReadRoboteqDriverState(); - - /** - * @brief Sends commands to the motors - * - * @param cmd command value in the range [-1000, 1000] - * - * @exception std::runtime_error if operation fails - */ - void SendRoboteqCmd(const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2); + DriverState ReadState() override; /** * @exception std::runtime_error if any operation returns error */ - void ResetRoboteqScript(); + void ResetScript() override; /** * @exception std::runtime_error if any operation returns error */ - void TurnOnEStop(); + void TurnOnEStop() override; /** * @exception std::runtime_error if any operation returns error */ - void TurnOffEStop(); + void TurnOffEStop() override; /** - * @brief Sends a safety stop command to the motor connected to channel 1 - * - * @exception std::runtime_error if any operation returns error + * @brief Adds a motor driver to the driver */ - void TurnOnSafetyStopChannel1(); + void AddMotorDriver( + const std::string name, std::shared_ptr motor_driver) override; /** - * @brief Sends a safety stop command to the motor connected to channel 2 + * @brief Returns a motor driver by name * - * @exception std::runtime_error if any operation returns error + * @exception std::runtime_error if motor driver with the given name does not exist */ - void TurnOnSafetyStopChannel2(); + std::shared_ptr GetMotorDriver(const std::string & name) override; -private: /** * @brief Blocking SDO write operation * @@ -150,6 +104,28 @@ class RoboteqDriver : public lely::canopen::LoopDriver template void SyncSDOWrite(const std::uint16_t index, const std::uint8_t subindex, const T data); + /** + * @brief Returns the last timestamp of the position data for the given channel + */ + timespec GetPositionTimestamp(const std::uint8_t channel) + { + std::lock_guard lck(position_timestamp_mtx_); + return last_position_timestamps_.at(channel); + } + + /** + * @brief Returns the last timestamp of the speed and current data for the given channel + */ + timespec GetSpeedCurrentTimestamp(const std::uint8_t channel) + { + std::lock_guard lck(speed_current_timestamp_mtx_); + return last_speed_current_timestamps_.at(channel); + } + + static constexpr std::uint8_t kChannel1 = 1; + static constexpr std::uint8_t kChannel2 = 2; + +private: void OnBoot( const lely::canopen::NmtState st, const char es, const std::string & what) noexcept override; void OnRpdoWrite(const std::uint16_t idx, const std::uint8_t subidx) noexcept override; @@ -167,10 +143,11 @@ class RoboteqDriver : public lely::canopen::LoopDriver std::atomic_bool heartbeat_timeout_; std::mutex position_timestamp_mtx_; - timespec last_position_timestamp_; + std::map last_position_timestamps_ = {{kChannel1, {}}, {kChannel2, {}}}; std::mutex speed_current_timestamp_mtx_; - timespec last_speed_current_timestamp_; + std::map last_speed_current_timestamps_ = { + {kChannel1, {}}, {kChannel2, {}}}; std::mutex flags_current_timestamp_mtx_; timespec flags_current_timestamp_; @@ -179,8 +156,45 @@ class RoboteqDriver : public lely::canopen::LoopDriver timespec last_voltages_temps_timestamp_; const std::chrono::milliseconds sdo_operation_timeout_ms_; + + std::map> motor_drivers_; +}; + +class RoboteqMotorDriver : public MotorDriverInterface +{ +public: + RoboteqMotorDriver(std::weak_ptr driver, const std::uint8_t channel) + : driver_(driver), channel_(channel) + { + } + + /** + * @brief Reads motor state data and saves last timestamps + */ + MotorDriverState ReadState() override; + + /** + * @brief Sends commands to the motors + * + * @param cmd command value in the range [-1000, 1000] + * + * @exception std::runtime_error if operation fails + */ + void SendCmdVel(const std::int32_t cmd) override; + + /** + * @brief Sends a safety stop command to the motor connected to channel 1 + * + * @exception std::runtime_error if any operation returns error + */ + void TurnOnSafetyStop() override; + +private: + std::weak_ptr driver_; + + const std::uint8_t channel_; }; -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_DRIVER_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_DRIVER_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp similarity index 89% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp index ceadba30..063df2da 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_ERROR_FILTER_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_ERROR_FILTER_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_ #include #include #include -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { class ErrorFilter @@ -102,6 +102,6 @@ class RoboteqErrorFilter std::map error_filters_; }; -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_ROBOTEQ_ERROR_FILTER_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ERROR_FILTER_HPP_ diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp new file mode 100644 index 00000000..ea088a56 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp @@ -0,0 +1,175 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_ + +#include +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +struct MotorNames +{ + static constexpr char LEFT[] = "left"; + static constexpr char RIGHT[] = "right"; +}; + +struct DriverNames +{ + static constexpr char DEFAULT[] = "default"; + static constexpr char FRONT[] = "front"; + static constexpr char REAR[] = "rear"; +}; + +struct MotorChannels +{ + static constexpr std::uint8_t LEFT = RoboteqDriver::kChannel2; + static constexpr std::uint8_t RIGHT = RoboteqDriver::kChannel1; +}; + +/** + * @brief Abstract class implementing RobotDriver for robots using Roboteq drivers. + * It uses canopen_manager for communication with Roboteq controllers. + * This class implements the activation procedure for controllers, which involves resetting the + * script and sending an initial 0 command. It also provides methods to get data feedback and send + * commands. Data is converted between raw Roboteq formats and SI units using + * roboteq_data_converters. + */ +class RoboteqRobotDriver : public RobotDriverInterface +{ +public: + RoboteqRobotDriver( + const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings, + const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000)); + + ~RoboteqRobotDriver() + { + drivers_.clear(); + Deinitialize(); + }; + + /** + * @brief Starts CAN communication and waits for boot to finish + * + * @exception std::runtime_error if boot fails + */ + void Initialize() override; + + /** + * @brief Deinitialize can communication + */ + void Deinitialize() override; + + /** + * @brief Activate procedure for Roboteq drivers - reset scripts and send 0 commands on both + * channels. Blocking function, takes around 2 seconds to finish + * + * @exception std::runtime_error if any procedure step fails + */ + void Activate() override; + + /** + * @brief Updates current communication state with Roboteq drivers + * + * @exception std::runtime_error if CAN error was detected + */ + void UpdateCommunicationState() override; + + /** + * @brief Updates current motors' state (position, velocity, current). + * + * @exception std::runtime_error if CAN error was detected + */ + void UpdateMotorsState() override; + + /** + * @brief Updates current Roboteq driver state (flags, temperatures, voltage, battery current) + * + * @exception std::runtime_error if CAN error was detected + */ + void UpdateDriversState() override; + + /** + * @brief Turns on Roboteq E-stop + * + * @exception std::runtime_error if any operation returns error + */ + void TurnOnEStop() override; + + /** + * @brief Turns off Roboteq E-stop + * + * @exception std::runtime_error if any operation returns error + */ + void TurnOffEStop() override; + + /** + * @brief Get data feedback from the driver + * + * @param name name of the data to get + * + * @return data feedback + * @exception std::runtime_error if data with the given name does not exist + */ + const DriverData & GetData(const std::string & name) override; + + bool CommunicationError() override; + +protected: + /** + * @brief This method defines driver objects and adds motor drivers for them. + */ + virtual void DefineDrivers() = 0; + + RoboteqVelocityCommandConverter & GetCmdVelConverter() { return roboteq_vel_cmd_converter_; } + CANopenSettings & GetCANopenSettings() { return canopen_settings_; } + CANopenManager & GetCANopenManager() { return canopen_manager_; } + + CANopenSettings canopen_settings_; + DrivetrainSettings drivetrain_settings_; + + CANopenManager canopen_manager_; + std::map> drivers_; + +private: + void SetMotorsStates( + DriverData & data, const MotorDriverState & left_state, const MotorDriverState & right_state, + const timespec & current_time); + void SetDriverState(DriverData & data, const DriverState & state, const timespec & current_time); + bool DataTimeout( + const timespec & current_time, const timespec & data_timestamp, + const std::chrono::milliseconds & timeout); + void BootDrivers(); + + bool initialized_ = false; + + std::map data_; + RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_; + + const std::chrono::milliseconds pdo_motor_states_timeout_ms_; + const std::chrono::milliseconds pdo_driver_state_timeout_ms_; + const std::chrono::milliseconds activate_wait_time_; +}; + +} // namespace husarion_ugv_hardware_interfaces + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_ROBOT_DRIVER_ROBOTEQ_ROBOT_DRIVER_HPP_ diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp new file mode 100644 index 00000000..7cc903a3 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp @@ -0,0 +1,120 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_E_STOP_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_E_STOP_HPP_ + +#include +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +/** + * @class EStopInterface + * @brief Abstract base class defining the interface for emergency stop detailed implementations. + */ +class EStopInterface +{ +public: + EStopInterface() {} + + virtual ~EStopInterface() = default; + + virtual bool ReadEStopState() = 0; + virtual void TriggerEStop() = 0; + virtual void ResetEStop() = 0; +}; + +/** + * @class EStop + * @brief Implements the emergency stop interface. + */ +class EStop : public EStopInterface +{ +public: + EStop( + std::shared_ptr gpio_controller, + std::shared_ptr roboteq_error_filter, + std::shared_ptr robot_driver, + std::shared_ptr robot_driver_write_mtx, std::function zero_velocity_check) + : EStopInterface(), + gpio_controller_(gpio_controller), + roboteq_error_filter_(roboteq_error_filter), + robot_driver_(robot_driver), + robot_driver_write_mtx_(robot_driver_write_mtx), + ZeroVelocityCheck(zero_velocity_check) {}; + + virtual ~EStop() override = default; + + /** + * @brief Checks the emergency stop state. + * + * 1. Check if ESTOP GPIO pin is not active. If is not it means that E-Stop is triggered by + * another device within the robot's system (e.g., Roboteq controller or Safety Board), + * disabling the software Watchdog is necessary to prevent an uncontrolled reset. + * 2. If there is a need, disable software Watchdog using + * GPIOControllerInterface::EStopTrigger method. + * 3. Return ESTOP GPIO pin state. + */ + bool ReadEStopState() override; + + /** + * @brief Triggers the emergency stop. + * + * 1. Interrupt the E-Stop resetting process if it is in progress. + * 2. Attempt to trigger the E-Stop using GPIO by disabling the software-controlled watchdog. + * 3. If successful, set e_stop_triggered_ to true; otherwise, throw a std::runtime_error + * exception. + * + * @throws std::runtime_error if triggering the E-stop using GPIO fails. + */ + void TriggerEStop() override; + + /** + * @brief Resets the emergency stop. + * + * 1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement + * after an E-stop state change. + * 2. Attempt to reset the E-Stop using GPIO by manipulating the ESTOP GPIO pin. This operation + * may take some time and can be interrupted by the E-Stop trigger process. + * 3. Set the clear_error flag to allow for clearing of Roboteq errors. + * 4. Confirm the E-Stop reset was successful with the ReadEStopState method. + * + * @throws EStopResetInterrupted if the E-stop reset operation was halted because the E-stop was + * triggered again. + * @throws std::runtime_error if an error occurs when trying to reset the E-stop using GPIO. + */ + void ResetEStop() override; + +protected: + std::shared_ptr gpio_controller_; + std::shared_ptr roboteq_error_filter_; + std::shared_ptr robot_driver_; + std::shared_ptr robot_driver_write_mtx_; + + std::function ZeroVelocityCheck; + + std::mutex e_stop_manipulation_mtx_; + std::atomic_bool e_stop_triggered_ = true; +}; + +} // namespace husarion_ugv_hardware_interfaces + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_E_STOP_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp similarity index 80% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp index e27a0b36..4b813e17 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp @@ -12,41 +12,51 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_ROS_INTERFACE_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_ROS_INTERFACE_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_ #include -#include #include +#include #include #include #include -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "rclcpp/rclcpp.hpp" -#include "realtime_tools/realtime_publisher.h" +#include +#include +#include -#include "std_msgs/msg/bool.hpp" -#include "std_srvs/srv/set_bool.hpp" -#include "std_srvs/srv/trigger.hpp" +#include +#include +#include -#include "panther_msgs/msg/driver_state.hpp" +#include "panther_msgs/msg/driver_state_named.hpp" #include "panther_msgs/msg/io_state.hpp" +#include "panther_msgs/msg/robot_driver_state.hpp" -#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp" using namespace std::placeholders; -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { using BoolMsg = std_msgs::msg::Bool; -using DriverStateMsg = panther_msgs::msg::DriverState; +using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; using IOStateMsg = panther_msgs::msg::IOState; +using DriverStateNamedMsg = panther_msgs::msg::DriverStateNamed; using SetBoolSrv = std_srvs::srv::SetBool; using TriggerSrv = std_srvs::srv::Trigger; +struct DriverCANErrors +{ + bool motor_states_data_timed_out; + bool driver_state_data_timed_out; + bool can_error; + bool heartbeat_timeout; +}; + struct CANErrors { bool error; @@ -55,17 +65,7 @@ struct CANErrors bool read_pdo_motor_states_error; bool read_pdo_driver_state_error; - bool front_motor_states_data_timed_out; - bool rear_motor_states_data_timed_out; - - bool front_driver_state_data_timed_out; - bool rear_driver_state_data_timed_out; - - bool front_can_error; - bool rear_can_error; - - bool front_heartbeat_timeout; - bool rear_heartbeat_timeout; + std::unordered_map driver_errors; }; /** @@ -141,7 +141,7 @@ class ROSServiceWrapper * @brief Class that takes care of additional ROS interface of panther system, such as publishing * driver state and providing service for clearing errors */ -class PantherSystemRosInterface +class SystemROSInterface { public: /** @@ -150,10 +150,10 @@ class PantherSystemRosInterface * @param node_name * @param node_options */ - PantherSystemRosInterface( + SystemROSInterface( const std::string & node_name, const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()); - ~PantherSystemRosInterface(); + ~SystemROSInterface(); /** * @brief Registers a new service server associated with a specific service type and callback on @@ -220,14 +220,21 @@ class PantherSystemRosInterface } /** - * @brief Updates fault flags, script flags, and runtime errors in the driver state msg + * @brief Updates fault flags, script flags, and runtime errors in the robot driver state msg + * + * @param name The name of the driver to update the flags for + * @param data The data to update the flags with */ - void UpdateMsgErrorFlags(const RoboteqData & front, const RoboteqData & rear); + void UpdateMsgErrorFlags(const std::string & name, const DriverData & data); /** - * @brief Updates parameters of the drivers: voltage, current and temperature + * @brief Updates parameters of the driver (voltage, current and temperature) in robot driver + * state msg + * + * @param name The name of the driver to update the parameters for + * @param state The data to update the parameters with */ - void UpdateMsgDriversStates(const DriverState & front, const DriverState & rear); + void UpdateMsgDriversStates(const std::string & name, const RoboteqDriverState & state); /** * @brief Updates the current state of communication errors and general error state @@ -236,11 +243,11 @@ class PantherSystemRosInterface void PublishEStopStateMsg(const bool e_stop); void PublishEStopStateIfChanged(const bool e_stop); - void PublishDriverState(); + void PublishRobotDriverState(); void InitializeAndPublishIOStateMsg(const std::unordered_map & io_state); void PublishIOState(const GPIOInfo & gpio_info); -private: +protected: /** * @brief Updates the IOState message and indicates whether its state has changed. * @@ -269,13 +276,25 @@ class PantherSystemRosInterface rclcpp::CallbackGroup::SharedPtr GetOrCreateNodeCallbackGroup( const unsigned group_id, rclcpp::CallbackGroupType callback_group_type); + /** + * @brief Retrieves the driver state message associated with the specified name. If the driver + * state is not found, it is created. + * + * @param robot_driver_state The robot driver state message to retrieve driver state from. + * @param name The name of the driver state to be retrieved. + * + * @return Reference to the driver state message associated with the specified name. + */ + DriverStateNamedMsg & GetDriverStateByName( + RobotDriverStateMsg & robot_driver_state, const std::string & name); + rclcpp::Node::SharedPtr node_; std::unordered_map callback_groups_; rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_; std::thread executor_thread_; - rclcpp::Publisher::SharedPtr driver_state_publisher_; - std::unique_ptr> + rclcpp::Publisher::SharedPtr driver_state_publisher_; + std::unique_ptr> realtime_driver_state_publisher_; rclcpp::Publisher::SharedPtr io_state_publisher_; @@ -289,6 +308,6 @@ class PantherSystemRosInterface std::vector service_wrappers_storage_; }; -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_ROS_INTERFACE_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_SYSTEM_ROS_INTERFACE_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp similarity index 57% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp index 362be65b..ac4e574c 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp @@ -12,31 +12,31 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_UGV_SYSTEM_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_UGV_SYSTEM_HPP_ -#include #include #include #include #include -#include "diagnostic_updater/diagnostic_status_wrapper.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" +#include +#include +#include +#include -#include "hardware_interface/handle.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include +#include +#include -#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp" -#include "panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp" -#include "panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp" -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { using return_type = hardware_interface::return_type; @@ -45,12 +45,21 @@ using StateInterface = hardware_interface::StateInterface; using CommandInterface = hardware_interface::CommandInterface; /** - * @brief Class that implements SystemInterface from ros2_control for Panther + * @brief Class that implements SystemInterface from ros2_control for Husarion UGV */ -class PantherSystem : public hardware_interface::SystemInterface +class UGVSystem : public hardware_interface::SystemInterface { public: - RCLCPP_SHARED_PTR_DEFINITIONS(PantherSystem) + RCLCPP_SHARED_PTR_DEFINITIONS(UGVSystem) + UGVSystem(const std::vector & joint_order) + : SystemInterface(), + joint_size_(joint_order.size()), + joint_order_(joint_order), + joints_names_sorted_(joint_size_) + { + } + + virtual ~UGVSystem() = default; CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) override; CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; @@ -72,63 +81,66 @@ class PantherSystem : public hardware_interface::SystemInterface void SortAndCheckJointNames(); void SetInitialValues(); void CheckInterfaces() const; - void ReadPantherVersion(); + void ReadDrivetrainSettings(); void ReadCANopenSettings(); + virtual void ReadCANopenSettingsDriverCANIDs() = 0; + void ReadInitializationActivationAttempts(); void ReadParametersAndCreateRoboteqErrorFilter(); void ReadDriverStatesUpdateFrequency(); - void ConfigureGPIOController(); - void ConfigureMotorsController(); - void ConfigureEStop(); + virtual void ConfigureGPIOController(); // virtual for mocking + void ConfigureRobotDriver(); + virtual void DefineRobotDriver() = 0; + virtual void ConfigureEStop(); // virtual for mocking void UpdateMotorsState(); void UpdateDriverState(); void UpdateEStopState(); - void UpdateHwStates(); - void UpdateMotorsStateDataTimedOut(); + virtual void UpdateHwStates() = 0; + virtual void UpdateMotorsStateDataTimedOut() = 0; // possible but needs changes in robot driver bool AreVelocityCommandsNearZero(); - bool IsPantherVersionAtLeast(const float version); - void UpdateDriverStateMsg(); - void UpdateFlagErrors(); - void UpdateDriverStateDataTimedOut(); + virtual void UpdateDriverStateMsg() = 0; + virtual void UpdateFlagErrors() = 0; // possible but needs changes in robot driver + virtual void UpdateDriverStateDataTimedOut() = 0; // possible but needs changes in robot driver - void HandlePDOWriteOperation(std::function pdo_write_operation); + void HandleRobotDriverWriteOperation(std::function write_operation); + virtual std::vector GetSpeedCommands() const = 0; void MotorsPowerEnable(const bool enable); - void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status); - void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status); + virtual void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) = 0; + virtual void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) = 0; - static constexpr size_t kJointsSize = 4; + const size_t joint_size_; // Currently only velocity command mode is supported - although Roboteq driver support position // and torque mode, in 2.1 firmware both modes aren't really stable and safe. // In torque mode sometimes after killing the software motor moves and it generally isn't well // tuned. Position mode also isn't really stable (reacts abruptly to spikes). If updating the // firmware to 2.1a will solve these issues, it may be worth to enable other modes. - std::array hw_commands_velocities_; + std::vector hw_commands_velocities_; - std::array hw_states_positions_; - std::array hw_states_velocities_; - std::array hw_states_efforts_; + std::vector hw_states_positions_; + std::vector hw_states_velocities_; + std::vector hw_states_efforts_; // Define expected joint order, so that it doesn't matter what order is defined in the URDF. // It is expected that the joint name should contain these specifiers. - static const inline std::array joint_order_ = {"fl", "fr", "rl", "rr"}; - std::array joints_names_sorted_; + const std::vector joint_order_; + std::vector joints_names_sorted_; std::shared_ptr gpio_controller_; - std::shared_ptr motors_controller_; + std::shared_ptr robot_driver_; std::shared_ptr e_stop_; DrivetrainSettings drivetrain_settings_; CANopenSettings canopen_settings_; - std::unique_ptr panther_system_ros_interface_; + std::unique_ptr system_ros_interface_; // Sometimes SDO errors can happen during initialization and activation of Roboteq drivers, // in these cases it is better to retry @@ -140,19 +152,17 @@ class PantherSystem : public hardware_interface::SystemInterface unsigned max_roboteq_initialization_attempts_; unsigned max_roboteq_activation_attempts_; - rclcpp::Logger logger_{rclcpp::get_logger("PantherSystem")}; + rclcpp::Logger logger_{rclcpp::get_logger("UGVSystem")}; rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; std::shared_ptr roboteq_error_filter_; - float panther_version_; - - std::shared_ptr motor_controller_write_mtx_; + std::shared_ptr robot_driver_write_mtx_; rclcpp::Time next_driver_state_update_time_{0, 0, RCL_ROS_TIME}; rclcpp::Duration driver_states_update_period_{0, 0}; }; -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_ROBOT_SYSTEM_UGV_SYSTEM_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/utils.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/utils.hpp similarity index 89% rename from panther_hardware_interfaces/include/panther_hardware_interfaces/utils.hpp rename to husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/utils.hpp index 9a53b792..b64e8522 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/utils.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_UTILS_HPP_ -#define PANTHER_HARDWARE_INTERFACES_UTILS_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_UTILS_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_UTILS_HPP_ #include #include #include #include -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { /** @@ -59,6 +59,6 @@ bool OperationWithAttempts( */ bool CheckIfJointNameContainValidSequence(const std::string & name, const std::string & sequence); -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces -#endif // PANTHER_HARDWARE_INTERFACES_UTILS_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_UTILS_HPP_ diff --git a/panther_hardware_interfaces/package.xml b/husarion_ugv_hardware_interfaces/package.xml similarity index 82% rename from panther_hardware_interfaces/package.xml rename to husarion_ugv_hardware_interfaces/package.xml index b6021749..96980fee 100644 --- a/panther_hardware_interfaces/package.xml +++ b/husarion_ugv_hardware_interfaces/package.xml @@ -1,9 +1,9 @@ - panther_hardware_interfaces + husarion_ugv_hardware_interfaces 2.1.2 - Hardware controller for Panther + Hardware controller for Husarion UGV Husarion Apache License 2.0 @@ -11,10 +11,11 @@ https://github.com/husarion/panther_ros https://github.com/husarion/panther_ros/issues - Maciej Stępień - Paweł Kowalski Jakub Delicat Paweł Irzyk + Dawid Kmak + Paweł Kowalski + Maciej Stępień ament_cmake @@ -30,9 +31,9 @@ generate_parameter_library geometry_msgs hardware_interface + husarion_ugv_utils imu_filter_madgwick panther_msgs - panther_utils phidgets_api rclcpp rclcpp_lifecycle @@ -42,6 +43,10 @@ tf2_geometry_msgs tf2_ros + ament_cmake_gtest + google-mock + ros_testing + ament_cmake diff --git a/panther_hardware_interfaces/src/panther_imu_sensor/panther_imu_sensor.cpp b/husarion_ugv_hardware_interfaces/src/phidget_imu_sensor/phidget_imu_sensor.cpp similarity index 86% rename from panther_hardware_interfaces/src/panther_imu_sensor/panther_imu_sensor.cpp rename to husarion_ugv_hardware_interfaces/src/phidget_imu_sensor/phidget_imu_sensor.cpp index 1cd14517..75f9405b 100644 --- a/panther_hardware_interfaces/src/panther_imu_sensor/panther_imu_sensor.cpp +++ b/husarion_ugv_hardware_interfaces/src/phidget_imu_sensor/phidget_imu_sensor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp" +#include "husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp" #include #include @@ -30,10 +30,10 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { -CallbackReturn PantherImuSensor::on_init(const hardware_interface::HardwareInfo & hardware_info) +CallbackReturn PhidgetImuSensor::on_init(const hardware_interface::HardwareInfo & hardware_info) { if (hardware_interface::SensorInterface::on_init(hardware_info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; @@ -52,7 +52,7 @@ CallbackReturn PantherImuSensor::on_init(const hardware_interface::HardwareInfo return CallbackReturn::SUCCESS; } -CallbackReturn PantherImuSensor::on_configure(const rclcpp_lifecycle::State &) +CallbackReturn PhidgetImuSensor::on_configure(const rclcpp_lifecycle::State &) { try { ReadObligatoryParams(); @@ -91,12 +91,12 @@ CallbackReturn PantherImuSensor::on_configure(const rclcpp_lifecycle::State &) return CallbackReturn::SUCCESS; } -CallbackReturn PantherImuSensor::on_cleanup(const rclcpp_lifecycle::State &) +CallbackReturn PhidgetImuSensor::on_cleanup(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn PantherImuSensor::on_activate(const rclcpp_lifecycle::State &) +CallbackReturn PhidgetImuSensor::on_activate(const rclcpp_lifecycle::State &) { rclcpp::NodeOptions ros_interface_options; @@ -108,9 +108,9 @@ CallbackReturn PantherImuSensor::on_activate(const rclcpp_lifecycle::State &) if (!spatial_) { spatial_ = std::make_unique( params_.serial, params_.hub_port, false, - std::bind(&PantherImuSensor::SpatialDataCallback, this, _1, _2, _3, _4), nullptr, - std::bind(&PantherImuSensor::SpatialAttachCallback, this), - std::bind(&PantherImuSensor::SpatialDetachCallback, this)); + std::bind(&PhidgetImuSensor::SpatialDataCallback, this, _1, _2, _3, _4), nullptr, + std::bind(&PhidgetImuSensor::SpatialAttachCallback, this), + std::bind(&PhidgetImuSensor::SpatialDetachCallback, this)); } imu_connected_ = true; @@ -131,22 +131,22 @@ CallbackReturn PantherImuSensor::on_activate(const rclcpp_lifecycle::State &) return CallbackReturn::SUCCESS; } -CallbackReturn PantherImuSensor::on_deactivate(const rclcpp_lifecycle::State &) +CallbackReturn PhidgetImuSensor::on_deactivate(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn PantherImuSensor::on_shutdown(const rclcpp_lifecycle::State &) +CallbackReturn PhidgetImuSensor::on_shutdown(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn PantherImuSensor::on_error(const rclcpp_lifecycle::State &) +CallbackReturn PhidgetImuSensor::on_error(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -std::vector PantherImuSensor::export_state_interfaces() +std::vector PhidgetImuSensor::export_state_interfaces() { std::vector state_interfaces; @@ -159,7 +159,7 @@ std::vector PantherImuSensor::export_state_interfaces() return state_interfaces; } -return_type PantherImuSensor::read( +return_type PhidgetImuSensor::read( const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) { if (!imu_connected_) { @@ -168,14 +168,14 @@ return_type PantherImuSensor::read( return return_type::OK; } -void PantherImuSensor::CheckSensorName() const +void PhidgetImuSensor::CheckSensorName() const { if (!info_.sensors.size()) { throw std::runtime_error("Sensor is not defined in URDF!"); } } -void PantherImuSensor::CheckStatesSize() const +void PhidgetImuSensor::CheckStatesSize() const { if (info_.sensors.at(0).state_interfaces.size() != kImuInterfacesSize) { throw std::runtime_error( @@ -185,7 +185,7 @@ void PantherImuSensor::CheckStatesSize() const } } -void PantherImuSensor::CheckInterfaces() const +void PhidgetImuSensor::CheckInterfaces() const { const auto names_start_iter = kImuInterfacesNames.begin(); const auto names_end_iter = kImuInterfacesNames.end(); @@ -201,7 +201,7 @@ void PantherImuSensor::CheckInterfaces() const } } -void PantherImuSensor::ReadObligatoryParams() +void PhidgetImuSensor::ReadObligatoryParams() { params_.serial = std::stoi(info_.hardware_parameters.at("serial")); params_.hub_port = std::stoi(info_.hardware_parameters.at("hub_port")); @@ -215,7 +215,7 @@ void PantherImuSensor::ReadObligatoryParams() } } -void PantherImuSensor::ReadCompassParams() +void PhidgetImuSensor::ReadCompassParams() { params_.cc_mag_field = hardware_interface::stod(info_.hardware_parameters.at("cc_mag_field")); params_.cc_offset0 = hardware_interface::stod(info_.hardware_parameters.at("cc_offset0")); @@ -232,7 +232,7 @@ void PantherImuSensor::ReadCompassParams() params_.cc_t5 = hardware_interface::stod(info_.hardware_parameters.at("cc_t5")); } -void PantherImuSensor::ReadMadgwickFilterParams() +void PhidgetImuSensor::ReadMadgwickFilterParams() { params_.gain = hardware_interface::stod(info_.hardware_parameters.at("gain")); params_.zeta = hardware_interface::stod(info_.hardware_parameters.at("zeta")); @@ -247,7 +247,7 @@ void PantherImuSensor::ReadMadgwickFilterParams() CheckMadgwickFilterWorldFrameParam(); } -void PantherImuSensor::CheckMadgwickFilterWorldFrameParam() +void PhidgetImuSensor::CheckMadgwickFilterWorldFrameParam() { const auto world_frame = info_.hardware_parameters.at("world_frame"); @@ -265,13 +265,13 @@ void PantherImuSensor::CheckMadgwickFilterWorldFrameParam() } } -void PantherImuSensor::SetInitialValues() +void PhidgetImuSensor::SetInitialValues() { imu_sensor_state_.resize( info_.sensors.at(0).state_interfaces.size(), std::numeric_limits::quiet_NaN()); } -void PantherImuSensor::Calibrate() +void PhidgetImuSensor::Calibrate() { spatial_->zero(); @@ -284,12 +284,12 @@ void PantherImuSensor::Calibrate() RCLCPP_INFO(logger_, "IMU sensor calibration completed."); } -bool PantherImuSensor::IsParamDefined(const std::string & param_name) const +bool PhidgetImuSensor::IsParamDefined(const std::string & param_name) const { return info_.hardware_parameters.find(param_name) != info_.hardware_parameters.end(); } -bool PantherImuSensor::AreParamsDefined(const std::unordered_set & params_names) const +bool PhidgetImuSensor::AreParamsDefined(const std::unordered_set & params_names) const { for (const auto & param_name : params_names) { if (!IsParamDefined(param_name)) { @@ -299,7 +299,7 @@ bool PantherImuSensor::AreParamsDefined(const std::unordered_set & return true; } -void PantherImuSensor::ConfigureCompassParams() +void PhidgetImuSensor::ConfigureCompassParams() { if (AreParamsDefined( {"cc_mag_field", "cc_offset0", "cc_offset1", "cc_offset2", "cc_gain0", "cc_gain1", @@ -316,7 +316,7 @@ void PantherImuSensor::ConfigureCompassParams() } } -void PantherImuSensor::ConfigureHeating() +void PhidgetImuSensor::ConfigureHeating() { if (IsParamDefined("heating_enabled")) { params_.heating_enabled = @@ -327,7 +327,7 @@ void PantherImuSensor::ConfigureHeating() } } -void PantherImuSensor::ConfigureMadgwickFilter() +void PhidgetImuSensor::ConfigureMadgwickFilter() { filter_ = std::make_unique(); filter_->setWorldFrame(world_frame_); @@ -335,7 +335,7 @@ void PantherImuSensor::ConfigureMadgwickFilter() filter_->setDriftBiasGain(params_.zeta); } -geometry_msgs::msg::Vector3 PantherImuSensor::ParseMagnitude(const double magnetic_field[3]) +geometry_msgs::msg::Vector3 PhidgetImuSensor::ParseMagnitude(const double magnetic_field[3]) { geometry_msgs::msg::Vector3 mag_fld; @@ -352,7 +352,7 @@ geometry_msgs::msg::Vector3 PantherImuSensor::ParseMagnitude(const double magnet return mag_fld; } -geometry_msgs::msg::Vector3 PantherImuSensor::ParseGyration(const double angular_rate[3]) +geometry_msgs::msg::Vector3 PhidgetImuSensor::ParseGyration(const double angular_rate[3]) { geometry_msgs::msg::Vector3 ang_vel; @@ -362,7 +362,7 @@ geometry_msgs::msg::Vector3 PantherImuSensor::ParseGyration(const double angular return ang_vel; } -geometry_msgs::msg::Vector3 PantherImuSensor::ParseAcceleration(const double acceleration[3]) +geometry_msgs::msg::Vector3 PhidgetImuSensor::ParseAcceleration(const double acceleration[3]) { geometry_msgs::msg::Vector3 lin_acc; @@ -372,7 +372,7 @@ geometry_msgs::msg::Vector3 PantherImuSensor::ParseAcceleration(const double acc return lin_acc; } -void PantherImuSensor::InitializeMadgwickAlgorithm( +void PhidgetImuSensor::InitializeMadgwickAlgorithm( const geometry_msgs::msg::Vector3 & mag_compensated, const geometry_msgs::msg::Vector3 & lin_acc, const rclcpp::Time & timestamp) { @@ -389,7 +389,7 @@ void PantherImuSensor::InitializeMadgwickAlgorithm( algorithm_initialized_ = true; } -void PantherImuSensor::RestartMadgwickAlgorithm() +void PhidgetImuSensor::RestartMadgwickAlgorithm() { if (!filter_) { return; @@ -399,7 +399,7 @@ void PantherImuSensor::RestartMadgwickAlgorithm() filter_->setOrientation(restarted_value, restarted_value, restarted_value, restarted_value); } -bool PantherImuSensor::IsIMUCalibrated(const geometry_msgs::msg::Vector3 & mag_compensated) +bool PhidgetImuSensor::IsIMUCalibrated(const geometry_msgs::msg::Vector3 & mag_compensated) { if (imu_calibrated_) { return true; @@ -409,18 +409,18 @@ bool PantherImuSensor::IsIMUCalibrated(const geometry_msgs::msg::Vector3 & mag_c return imu_calibrated_; } -bool PantherImuSensor::IsVectorFinite(const geometry_msgs::msg::Vector3 & vec) +bool PhidgetImuSensor::IsVectorFinite(const geometry_msgs::msg::Vector3 & vec) { return std::isfinite(vec.x) && std::isfinite(vec.y) && std::isfinite(vec.z); } -bool PantherImuSensor::IsMagnitudeSynchronizedWithAccelerationAndGyration( +bool PhidgetImuSensor::IsMagnitudeSynchronizedWithAccelerationAndGyration( const geometry_msgs::msg::Vector3 & mag_compensated) { return IsVectorFinite(mag_compensated); } -void PantherImuSensor::SpatialDataCallback( +void PhidgetImuSensor::SpatialDataCallback( const double acceleration[3], const double angular_rate[3], const double magnetic_field[3], const double timestamp) { @@ -475,14 +475,14 @@ void PantherImuSensor::SpatialDataCallback( UpdateAllStatesValues(ang_vel, lin_acc); } -void PantherImuSensor::SpatialAttachCallback() +void PhidgetImuSensor::SpatialAttachCallback() { RCLCPP_INFO(logger_, "IMU sensor has successfully attached and is now connected."); imu_connected_ = true; on_activate(rclcpp_lifecycle::State{}); } -void PantherImuSensor::SpatialDetachCallback() +void PhidgetImuSensor::SpatialDetachCallback() { RCLCPP_WARN( logger_, @@ -495,7 +495,7 @@ void PantherImuSensor::SpatialDetachCallback() on_deactivate(rclcpp_lifecycle::State{}); } -void PantherImuSensor::UpdateMadgwickAlgorithm( +void PhidgetImuSensor::UpdateMadgwickAlgorithm( const geometry_msgs::msg::Vector3 & ang_vel, const geometry_msgs::msg::Vector3 & lin_acc, const geometry_msgs::msg::Vector3 & mag_compensated, const double dt) { @@ -504,7 +504,7 @@ void PantherImuSensor::UpdateMadgwickAlgorithm( mag_compensated.y, mag_compensated.z, dt); } -void PantherImuSensor::UpdateMadgwickAlgorithmIMU( +void PhidgetImuSensor::UpdateMadgwickAlgorithmIMU( const geometry_msgs::msg::Vector3 & ang_vel, const geometry_msgs::msg::Vector3 & lin_acc, const double dt) { @@ -512,7 +512,7 @@ void PantherImuSensor::UpdateMadgwickAlgorithmIMU( ang_vel.x, ang_vel.y, ang_vel.z, lin_acc.x, lin_acc.y, lin_acc.z, dt); } -void PantherImuSensor::UpdateAccelerationAndGyrationStateValues( +void PhidgetImuSensor::UpdateAccelerationAndGyrationStateValues( const geometry_msgs::msg::Vector3 & ang_vel, const geometry_msgs::msg::Vector3 & lin_acc) { imu_sensor_state_[angular_velocity_x] = ang_vel.x; @@ -532,7 +532,7 @@ void PantherImuSensor::UpdateAccelerationAndGyrationStateValues( } } -void PantherImuSensor::UpdateAllStatesValues( +void PhidgetImuSensor::UpdateAllStatesValues( const geometry_msgs::msg::Vector3 & ang_vel, const geometry_msgs::msg::Vector3 & lin_acc) { filter_->getOrientation( @@ -542,14 +542,14 @@ void PantherImuSensor::UpdateAllStatesValues( UpdateAccelerationAndGyrationStateValues(ang_vel, lin_acc); } -void PantherImuSensor::SetStateValuesToNans() +void PhidgetImuSensor::SetStateValuesToNans() { std::fill( imu_sensor_state_.begin(), imu_sensor_state_.end(), std::numeric_limits::quiet_NaN()); } -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - panther_hardware_interfaces::PantherImuSensor, hardware_interface::SensorInterface) + husarion_ugv_hardware_interfaces::PhidgetImuSensor, hardware_interface::SensorInterface) diff --git a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp similarity index 60% rename from panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp rename to husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp index fdb6ba2a..a90ac92f 100644 --- a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp" #include #include @@ -24,10 +24,14 @@ #include "gpiod.hpp" -namespace panther_hardware_interfaces +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/types.hpp" + +namespace husarion_ugv_hardware_interfaces { -Watchdog::Watchdog(std::shared_ptr gpio_driver) : gpio_driver_(std::move(gpio_driver)) +Watchdog::Watchdog(std::shared_ptr gpio_driver) +: gpio_driver_(std::move(gpio_driver)) { if (!gpio_driver_->IsPinAvailable(watchdog_pin_)) { throw std::runtime_error("Watchdog pin is not configured."); @@ -98,9 +102,17 @@ bool GPIOControllerInterface::IsPinAvailable(const GPIOPin pin) const return gpio_driver_->IsPinAvailable(pin); } -void GPIOControllerPTH12X::Start() +GPIOController::GPIOController(std::shared_ptr gpio_driver) +{ + gpio_driver_ = gpio_driver; + + if (!gpio_driver_) { + throw std::runtime_error("GPIO driver is not initialized."); + } +} + +void GPIOController::Start() { - gpio_driver_ = std::make_shared(gpio_config_info_storage_); gpio_driver_->GPIOMonitorEnable(true, 60); gpio_driver_->SetPinValue(GPIOPin::VMOT_ON, true); @@ -109,14 +121,14 @@ void GPIOControllerPTH12X::Start() watchdog_ = std::make_unique(gpio_driver_); } -void GPIOControllerPTH12X::EStopTrigger() +void GPIOController::EStopTrigger() { if (!watchdog_->TurnOff()) { throw std::runtime_error("Can't stop watchdog thread"); } } -void GPIOControllerPTH12X::EStopReset() +void GPIOController::EStopReset() { const auto e_stop_pin = GPIOPin::E_STOP_RESET; bool e_stop_state = !gpio_driver_->IsPinActive(e_stop_pin); @@ -150,39 +162,39 @@ void GPIOControllerPTH12X::EStopReset() } } -bool GPIOControllerPTH12X::MotorPowerEnable(const bool enable) +bool GPIOController::MotorPowerEnable(const bool enable) { return gpio_driver_->SetPinValue(GPIOPin::DRIVER_EN, enable); }; -bool GPIOControllerPTH12X::AUXPowerEnable(const bool enable) +bool GPIOController::AUXPowerEnable(const bool enable) { return gpio_driver_->SetPinValue(GPIOPin::AUX_PW_EN, enable); }; -bool GPIOControllerPTH12X::FanEnable(const bool enable) +bool GPIOController::FanEnable(const bool enable) { return gpio_driver_->SetPinValue(GPIOPin::FAN_SW, enable); }; -bool GPIOControllerPTH12X::DigitalPowerEnable(const bool enable) +bool GPIOController::DigitalPowerEnable(const bool enable) { return gpio_driver_->SetPinValue(GPIOPin::VDIG_OFF, !enable); }; -bool GPIOControllerPTH12X::ChargerEnable(const bool enable) +bool GPIOController::ChargerEnable(const bool enable) { return gpio_driver_->SetPinValue(GPIOPin::CHRG_DISABLE, !enable); } -bool GPIOControllerPTH12X::LEDControlEnable(const bool enable) +bool GPIOController::LEDControlEnable(const bool enable) { // pin_validation_wait_time=10ms used due to slow pin state transition // on pin loaded by high 100nF capacity in SBC Overlay v1.4 return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable, std::chrono::milliseconds(10)); } -std::unordered_map GPIOControllerPTH12X::QueryControlInterfaceIOStates() const +std::unordered_map GPIOController::QueryControlInterfaceIOStates() const { std::unordered_map io_state; @@ -199,14 +211,38 @@ std::unordered_map GPIOControllerPTH12X::QueryControlInterfaceIOS return io_state; } -void GPIOControllerPTH12X::InterruptEStopReset() +const std::vector GPIOController::gpio_config_info_storage_ = { + GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, + GPIOInfo{ + GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT, false, gpiod::line::value::ACTIVE}, + GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::GPIN1, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::GPIN2, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT}, + GPIOInfo{GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}, + GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT, true}, + GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true}, +}; + +const std::vector & GPIOController::GetGPIOConfigInfoStorage() +{ + return gpio_config_info_storage_; +} + +void GPIOController::InterruptEStopReset() { std::lock_guard lck(e_stop_cv_mtx_); should_abort_e_stop_reset_ = true; e_stop_cv_.notify_one(); } -bool GPIOControllerPTH12X::WaitFor(std::chrono::milliseconds timeout) +bool GPIOController::WaitFor(std::chrono::milliseconds timeout) { std::unique_lock lck(e_stop_cv_mtx_); @@ -217,77 +253,4 @@ bool GPIOControllerPTH12X::WaitFor(std::chrono::milliseconds timeout) return !interrupted; } -void GPIOControllerPTH10X::Start() -{ - gpio_driver_ = std::make_shared(gpio_config_info_storage_); - gpio_driver_->GPIOMonitorEnable(true, 60); - - gpio_driver_->SetPinValue(GPIOPin::MOTOR_ON, true); -} - -void GPIOControllerPTH10X::EStopTrigger() -{ - throw std::runtime_error( - "This robot version does not support this functionality. Trying to set safety stop using CAN " - "command."); -} - -void GPIOControllerPTH10X::EStopReset() -{ - if (!gpio_driver_->IsPinActive(GPIOPin::STAGE2_INPUT)) { - throw std::runtime_error( - "Motors are not powered up. Please verify if the main switch is in the 'STAGE2' position."); - } -} - -bool GPIOControllerPTH10X::MotorPowerEnable(const bool enable) -{ - if (enable && !gpio_driver_->IsPinActive(GPIOPin::STAGE2_INPUT)) { - throw std::runtime_error( - "Motors are not powered up. Please verify if the main switch is in the 'STAGE2' position."); - } - - return gpio_driver_->SetPinValue(GPIOPin::MOTOR_ON, enable); -} - -bool GPIOControllerPTH10X::AUXPowerEnable(const bool /* enable */) -{ - throw std::runtime_error("This robot version does not support this functionality."); -}; - -bool GPIOControllerPTH10X::FanEnable(const bool /* enable */) -{ - throw std::runtime_error("This robot version does not support this functionality."); -} - -bool GPIOControllerPTH10X::DigitalPowerEnable(const bool /* enable */) -{ - throw std::runtime_error("This robot version does not support this functionality."); -}; - -bool GPIOControllerPTH10X::ChargerEnable(const bool /* enable */) -{ - throw std::runtime_error("This robot version does not support this functionality."); -} - -bool GPIOControllerPTH10X::LEDControlEnable(const bool enable) -{ - return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable); -} - -std::unordered_map GPIOControllerPTH10X::QueryControlInterfaceIOStates() const -{ - std::unordered_map io_state; - - io_state.emplace(GPIOPin::AUX_PW_EN, true); - io_state.emplace(GPIOPin::CHRG_SENSE, false); - io_state.emplace(GPIOPin::CHRG_DISABLE, false); - io_state.emplace(GPIOPin::VDIG_OFF, false); - io_state.emplace(GPIOPin::FAN_SW, false); - io_state.emplace(GPIOPin::SHDN_INIT, false); - io_state.emplace(GPIOPin::MOTOR_ON, gpio_driver_->IsPinActive(GPIOPin::MOTOR_ON)); - - return io_state; -} - -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp similarity index 95% rename from panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp rename to husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp index 48126889..db050d97 100644 --- a/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp" #include #include @@ -29,9 +29,9 @@ #include "gpiod.hpp" -#include "panther_utils/configure_rt.hpp" +#include "husarion_ugv_utils/configure_rt.hpp" -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { GPIODriver::GPIODriver(std::vector gpio_info_storage) @@ -78,7 +78,7 @@ void GPIODriver::ConfigureEdgeEventCallback(const std::function GPIODriver::CreateLineRequest(gpiod::chip & chip) { auto request_builder = chip.prepare_request(); - request_builder.set_consumer("panther_hardware_interfaces"); + request_builder.set_consumer("husarion_ugv_hardware_interfaces"); for (GPIOInfo & gpio_info : gpio_info_storage_) { ConfigureLineRequest(chip, request_builder, gpio_info); @@ -230,7 +230,7 @@ void GPIODriver::GPIOMonitorOn() void GPIODriver::MonitorAsyncEvents() { if (use_rt_) { - panther_utils::ConfigureRT(gpio_monit_thread_sched_priority_); + husarion_ugv_utils::ConfigureRT(gpio_monit_thread_sched_priority_); } auto edge_event_buffer = gpiod::edge_event_buffer(edge_event_buffer_size_); @@ -316,4 +316,4 @@ GPIOPin GPIODriver::GetPinFromOffset(const gpiod::line::offset & offset) const "Pin with offset " + std::to_string(offset) + " not found in GPIO info storage."); } -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/lynx_system.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/lynx_system.cpp new file mode 100644 index 00000000..169f8c59 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/src/robot_system/lynx_system.cpp @@ -0,0 +1,177 @@ +// 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 "husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp" + +#include +#include +#include + +#include "diagnostic_updater/diagnostic_status_wrapper.hpp" +#include "rclcpp/logging.hpp" + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp" + +#include "husarion_ugv_utils/diagnostics.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +void LynxSystem::ReadCANopenSettingsDriverCANIDs() +{ + const auto driver_can_id = std::stoi(info_.hardware_parameters["driver_can_id"]); + canopen_settings_.driver_can_ids.emplace(DriverNames::DEFAULT, driver_can_id); +} + +void LynxSystem::DefineRobotDriver() +{ + robot_driver_ = std::make_shared(canopen_settings_, drivetrain_settings_); +} + +void LynxSystem::UpdateHwStates() +{ + const auto data = robot_driver_->GetData(DriverNames::DEFAULT); + + const auto left = data.GetMotorState(MotorChannels::LEFT); + const auto right = data.GetMotorState(MotorChannels::RIGHT); + + hw_states_positions_[0] = left.GetPosition(); + hw_states_positions_[1] = right.GetPosition(); + hw_states_positions_[2] = left.GetPosition(); + hw_states_positions_[3] = right.GetPosition(); + + hw_states_velocities_[0] = left.GetVelocity(); + hw_states_velocities_[1] = right.GetVelocity(); + hw_states_velocities_[2] = left.GetVelocity(); + hw_states_velocities_[3] = right.GetVelocity(); + + hw_states_efforts_[0] = left.GetTorque(); + hw_states_efforts_[1] = right.GetTorque(); + hw_states_efforts_[2] = left.GetTorque(); + hw_states_efforts_[3] = right.GetTorque(); +} + +void LynxSystem::UpdateMotorsStateDataTimedOut() +{ + if (robot_driver_->GetData(DriverNames::DEFAULT).IsMotorStatesDataTimedOut()) { + RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO motor state data timeout."); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); + } else { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, false); + } +} + +void LynxSystem::UpdateDriverStateMsg() +{ + const auto driver_data = robot_driver_->GetData(DriverNames::DEFAULT); + + system_ros_interface_->UpdateMsgDriversStates(DriverNames::DEFAULT, driver_data.GetDriverState()); + system_ros_interface_->UpdateMsgErrorFlags(DriverNames::DEFAULT, driver_data); + + CANErrors can_errors; + can_errors.error = roboteq_error_filter_->IsError(); + can_errors.write_pdo_cmds_error = roboteq_error_filter_->IsError(ErrorsFilterIds::WRITE_PDO_CMDS); + can_errors.read_pdo_motor_states_error = + roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_MOTOR_STATES); + can_errors.read_pdo_driver_state_error = + roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_DRIVER_STATE); + + DriverCANErrors driver_can_errors; + driver_can_errors.motor_states_data_timed_out = driver_data.IsMotorStatesDataTimedOut(); + driver_can_errors.driver_state_data_timed_out = driver_data.IsDriverStateDataTimedOut(); + driver_can_errors.can_error = driver_data.IsCANError(); + driver_can_errors.heartbeat_timeout = driver_data.IsHeartbeatTimeout(); + + can_errors.driver_errors.emplace(DriverNames::DEFAULT, driver_can_errors); + + system_ros_interface_->UpdateMsgErrors(can_errors); +} + +void LynxSystem::UpdateFlagErrors() +{ + const auto driver_data = robot_driver_->GetData(DriverNames::DEFAULT); + if (driver_data.IsFlagError()) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, steady_clock_, 1000, + "Error state on the default driver: " << driver_data.GetFlagErrorLog()); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, true); + + HandleRobotDriverWriteOperation([this] { robot_driver_->AttemptErrorFlagReset(); }); + } else { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); + } +} + +void LynxSystem::UpdateDriverStateDataTimedOut() +{ + if (robot_driver_->GetData(DriverNames::DEFAULT).IsDriverStateDataTimedOut()) { + RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO driver state timeout."); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); + } else { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, false); + } +} + +void LynxSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; + std::string message{"No error detected."}; + + const auto driver_data = robot_driver_->GetData(DriverNames::DEFAULT); + if (driver_data.IsError()) { + level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; + message = "Error detected."; + + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue( + status, driver_data.GetErrorMap(), "Driver error: "); + } + + if (roboteq_error_filter_->IsError()) { + level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; + message = "Error detected."; + + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue( + status, roboteq_error_filter_->GetErrorMap(), "", " error"); + } + + status.summary(level, message); +} + +void LynxSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; + std::string message{"Panther system status monitoring."}; + + const auto driver_state = robot_driver_->GetData(DriverNames::DEFAULT).GetDriverState(); + + status.add("Default driver voltage (V)", driver_state.GetVoltage()); + status.add("Default driver current (A)", driver_state.GetCurrent()); + status.add("Default driver temperature (\u00B0C)", driver_state.GetTemperature()); + status.add( + "Default driver heatsink temperature (\u00B0C)", driver_state.GetHeatsinkTemperature()); + + status.summary(level, message); +} + +std::vector LynxSystem::GetSpeedCommands() const +{ + return { + static_cast(hw_commands_velocities_[0]), static_cast(hw_commands_velocities_[1])}; +} + +} // namespace husarion_ugv_hardware_interfaces + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + husarion_ugv_hardware_interfaces::LynxSystem, hardware_interface::SystemInterface) diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/panther_system.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/panther_system.cpp new file mode 100644 index 00000000..18a29560 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/src/robot_system/panther_system.cpp @@ -0,0 +1,221 @@ +// 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 "husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp" + +#include +#include +#include + +#include "diagnostic_updater/diagnostic_status_wrapper.hpp" +#include "rclcpp/logging.hpp" + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp" + +#include "husarion_ugv_utils/diagnostics.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +void PantherSystem::ReadCANopenSettingsDriverCANIDs() +{ + const auto front_driver_can_id = std::stoi(info_.hardware_parameters["front_driver_can_id"]); + const auto rear_driver_can_id = std::stoi(info_.hardware_parameters["rear_driver_can_id"]); + canopen_settings_.driver_can_ids.emplace(DriverNames::FRONT, front_driver_can_id); + canopen_settings_.driver_can_ids.emplace(DriverNames::REAR, rear_driver_can_id); +} + +void PantherSystem::DefineRobotDriver() +{ + robot_driver_ = std::make_shared(canopen_settings_, drivetrain_settings_); +} + +void PantherSystem::UpdateHwStates() +{ + const auto front_data = robot_driver_->GetData(DriverNames::FRONT); + const auto rear_data = robot_driver_->GetData(DriverNames::REAR); + + const auto fl_motor_state = front_data.GetMotorState(MotorChannels::LEFT); + const auto fr_motor_state = front_data.GetMotorState(MotorChannels::RIGHT); + const auto rl_motor_state = rear_data.GetMotorState(MotorChannels::LEFT); + const auto rr_motor_state = rear_data.GetMotorState(MotorChannels::RIGHT); + + hw_states_positions_[0] = fl_motor_state.GetPosition(); + hw_states_positions_[1] = fr_motor_state.GetPosition(); + hw_states_positions_[2] = rl_motor_state.GetPosition(); + hw_states_positions_[3] = rr_motor_state.GetPosition(); + + hw_states_velocities_[0] = fl_motor_state.GetVelocity(); + hw_states_velocities_[1] = fr_motor_state.GetVelocity(); + hw_states_velocities_[2] = rl_motor_state.GetVelocity(); + hw_states_velocities_[3] = rr_motor_state.GetVelocity(); + + hw_states_efforts_[0] = fl_motor_state.GetTorque(); + hw_states_efforts_[1] = fr_motor_state.GetTorque(); + hw_states_efforts_[2] = rl_motor_state.GetTorque(); + hw_states_efforts_[3] = rr_motor_state.GetTorque(); +} + +void PantherSystem::UpdateMotorsStateDataTimedOut() +{ + if ( + robot_driver_->GetData(DriverNames::FRONT).IsMotorStatesDataTimedOut() || + robot_driver_->GetData(DriverNames::REAR).IsMotorStatesDataTimedOut()) { + RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO motor state data timeout."); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); + } else { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, false); + } +} + +void PantherSystem::UpdateDriverStateMsg() +{ + const auto front_driver_data = robot_driver_->GetData(DriverNames::FRONT); + const auto rear_driver_data = robot_driver_->GetData(DriverNames::REAR); + + system_ros_interface_->UpdateMsgDriversStates( + DriverNames::FRONT, front_driver_data.GetDriverState()); + system_ros_interface_->UpdateMsgDriversStates( + DriverNames::REAR, rear_driver_data.GetDriverState()); + system_ros_interface_->UpdateMsgErrorFlags(DriverNames::FRONT, front_driver_data); + system_ros_interface_->UpdateMsgErrorFlags(DriverNames::REAR, rear_driver_data); + + CANErrors can_errors; + can_errors.error = roboteq_error_filter_->IsError(); + can_errors.write_pdo_cmds_error = roboteq_error_filter_->IsError(ErrorsFilterIds::WRITE_PDO_CMDS); + can_errors.read_pdo_motor_states_error = + roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_MOTOR_STATES); + can_errors.read_pdo_driver_state_error = + roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_DRIVER_STATE); + + DriverCANErrors front_driver_can_errors; + DriverCANErrors rear_driver_can_errors; + + front_driver_can_errors.motor_states_data_timed_out = + front_driver_data.IsMotorStatesDataTimedOut(); + front_driver_can_errors.driver_state_data_timed_out = + front_driver_data.IsDriverStateDataTimedOut(); + front_driver_can_errors.can_error = front_driver_data.IsCANError(); + front_driver_can_errors.heartbeat_timeout = front_driver_data.IsHeartbeatTimeout(); + + rear_driver_can_errors.motor_states_data_timed_out = rear_driver_data.IsMotorStatesDataTimedOut(); + rear_driver_can_errors.driver_state_data_timed_out = rear_driver_data.IsDriverStateDataTimedOut(); + rear_driver_can_errors.can_error = rear_driver_data.IsCANError(); + rear_driver_can_errors.heartbeat_timeout = rear_driver_data.IsHeartbeatTimeout(); + + can_errors.driver_errors.emplace(DriverNames::FRONT, front_driver_can_errors); + can_errors.driver_errors.emplace(DriverNames::REAR, rear_driver_can_errors); + + system_ros_interface_->UpdateMsgErrors(can_errors); +} + +void PantherSystem::UpdateFlagErrors() +{ + const auto front_driver_data = robot_driver_->GetData(DriverNames::FRONT); + const auto rear_driver_data = robot_driver_->GetData(DriverNames::REAR); + if (front_driver_data.IsFlagError() || rear_driver_data.IsFlagError()) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, steady_clock_, 1000, + "Error state on one of the drivers:\n" + << "\tFront: " << front_driver_data.GetFlagErrorLog() + << "\tRear: " << rear_driver_data.GetFlagErrorLog()); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, true); + + HandleRobotDriverWriteOperation([this] { robot_driver_->AttemptErrorFlagReset(); }); + } else { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); + } +} + +void PantherSystem::UpdateDriverStateDataTimedOut() +{ + if ( + robot_driver_->GetData(DriverNames::FRONT).IsDriverStateDataTimedOut() || + robot_driver_->GetData(DriverNames::REAR).IsDriverStateDataTimedOut()) { + RCLCPP_WARN_STREAM_THROTTLE(logger_, steady_clock_, 1000, "PDO driver state timeout."); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); + } else { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, false); + } +} + +void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; + std::string message{"No error detected."}; + + const auto front_driver_data = robot_driver_->GetData(DriverNames::FRONT); + if (front_driver_data.IsError()) { + level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; + message = "Error detected."; + + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue( + status, front_driver_data.GetErrorMap(), "Front driver error: "); + } + + const auto rear_driver_data = robot_driver_->GetData(DriverNames::REAR); + if (rear_driver_data.IsError()) { + level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; + message = "Error detected."; + + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue( + status, rear_driver_data.GetErrorMap(), "Rear driver error: "); + } + + if (roboteq_error_filter_->IsError()) { + level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; + message = "Error detected."; + + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue( + status, roboteq_error_filter_->GetErrorMap(), "", " error"); + } + + status.summary(level, message); +} + +void PantherSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; + std::string message{"Panther system status monitoring."}; + + const auto front_driver_state = robot_driver_->GetData(DriverNames::FRONT).GetDriverState(); + const auto rear_driver_state = robot_driver_->GetData(DriverNames::REAR).GetDriverState(); + + auto driver_states_with_names = { + std::make_pair(std::string("Front"), front_driver_state), + std::make_pair(std::string("Rear"), rear_driver_state)}; + + for (const auto & [driver_name, driver_state] : driver_states_with_names) { + status.add(driver_name + " driver voltage (V)", driver_state.GetVoltage()); + status.add(driver_name + " driver current (A)", driver_state.GetCurrent()); + status.add(driver_name + " driver temperature (\u00B0C)", driver_state.GetTemperature()); + status.add( + driver_name + " driver heatsink temperature (\u00B0C)", + driver_state.GetHeatsinkTemperature()); + } + status.summary(level, message); +} + +std::vector PantherSystem::GetSpeedCommands() const +{ + return { + static_cast(hw_commands_velocities_[0]), static_cast(hw_commands_velocities_[1]), + static_cast(hw_commands_velocities_[2]), static_cast(hw_commands_velocities_[3])}; +} + +} // namespace husarion_ugv_hardware_interfaces + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + husarion_ugv_hardware_interfaces::PantherSystem, hardware_interface::SystemInterface) diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/canopen_manager.cpp similarity index 58% rename from panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/canopen_manager.cpp index 18f4117c..4b98a7f7 100644 --- a/panther_hardware_interfaces/src/panther_system/motors_controller/canopen_controller.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/canopen_manager.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" #include #include -#include #include #include #include @@ -25,17 +24,17 @@ #include "ament_index_cpp/get_package_share_directory.hpp" -#include "panther_utils/configure_rt.hpp" +#include "husarion_ugv_utils/configure_rt.hpp" -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { -CANopenController::CANopenController(const CANopenSettings & canopen_settings) +CANopenManager::CANopenManager(const CANopenSettings & canopen_settings) : canopen_settings_(canopen_settings) { } -void CANopenController::Initialize() +void CANopenManager::Initialize() { if (initialized_) { return; @@ -43,24 +42,33 @@ void CANopenController::Initialize() canopen_communication_started_.store(false); - canopen_communication_thread_ = std::thread([this]() { - try { - panther_utils::ConfigureRT(kCANopenThreadSchedPriority); - } catch (const std::runtime_error & e) { - std::cerr << "An exception occurred while configuring RT: " << e.what() << std::endl - << "Continuing with regular thread settings (it may have a negative impact on the " - "performance)." - << std::endl; - } + try { + husarion_ugv_utils::ConfigureRT(kCANopenThreadSchedPriority); + } catch (const std::runtime_error & e) { + std::cerr << "An exception occurred while configuring RT: " << e.what() << std::endl + << "Continuing with regular thread settings (it may have a negative impact on the " + "performance)." + << std::endl; + } - try { - InitializeCANCommunication(); - } catch (const std::system_error & e) { - std::cerr << "An exception occurred while initializing CAN: " << e.what() << std::endl; - NotifyCANCommunicationStarted(false); - return; - } + try { + InitializeCANCommunication(); + } catch (const std::system_error & e) { + std::cerr << "An exception occurred while initializing CAN: " << e.what() << std::endl; + NotifyCANCommunicationStarted(false); + return; + } + initialized_ = true; +} + +void CANopenManager::Activate() +{ + if (!initialized_) { + throw std::runtime_error("CANopenManager not initialized."); + } + + canopen_communication_thread_ = std::thread([this]() { NotifyCANCommunicationStarted(true); try { @@ -78,15 +86,11 @@ void CANopenController::Initialize() } if (!canopen_communication_started_.load()) { - throw std::runtime_error("CAN communication not initialized."); + throw std::runtime_error("CAN communication not activated."); } - - BootDrivers(); - - initialized_ = true; } -void CANopenController::Deinitialize() +void CANopenManager::Deinitialize() { // Deinitialization should be done regardless of the initialized_ state - in case some operation // during initialization fails, it is still necessary to do the cleanup @@ -101,8 +105,6 @@ void CANopenController::Deinitialize() canopen_communication_started_.store(false); - rear_driver_.reset(); - front_driver_.reset(); master_.reset(); chan_.reset(); ctrl_.reset(); @@ -115,7 +117,7 @@ void CANopenController::Deinitialize() initialized_ = false; } -void CANopenController::InitializeCANCommunication() +void CANopenManager::InitializeCANCommunication() { lely::io::IoGuard io_guard; @@ -135,22 +137,17 @@ void CANopenController::InitializeCANCommunication() // dcfgen canopen_configuration.yaml -r // dcfgen comes with lely, -r option tells to enable remote PDO mapping std::string master_dcf_path = std::filesystem::path(ament_index_cpp::get_package_share_directory( - "panther_hardware_interfaces")) / + "husarion_ugv_hardware_interfaces")) / "config" / "master.dcf"; master_ = std::make_shared( *timer_, *chan_, master_dcf_path, "", canopen_settings_.master_can_id); - front_driver_ = std::make_shared( - master_, canopen_settings_.front_driver_can_id, canopen_settings_.sdo_operation_timeout_ms); - rear_driver_ = std::make_shared( - master_, canopen_settings_.rear_driver_can_id, canopen_settings_.sdo_operation_timeout_ms); - // Start the NMT service of the master by pretending to receive a 'reset node' command. master_->Reset(); } -void CANopenController::NotifyCANCommunicationStarted(const bool result) +void CANopenManager::NotifyCANCommunicationStarted(const bool result) { { std::lock_guard lck_g(canopen_communication_started_mtx_); @@ -159,32 +156,4 @@ void CANopenController::NotifyCANCommunicationStarted(const bool result) canopen_communication_started_cond_.notify_all(); } -void CANopenController::BootDrivers() -{ - try { - auto front_driver_future = front_driver_->Boot(); - auto rear_driver_future = rear_driver_->Boot(); - - auto front_driver_status = front_driver_future.wait_for(std::chrono::seconds(5)); - auto rear_driver_status = rear_driver_future.wait_for(std::chrono::seconds(5)); - - if ( - front_driver_status == std::future_status::ready && - rear_driver_status == std::future_status::ready) { - try { - front_driver_future.get(); - rear_driver_future.get(); - } catch (const std::exception & e) { - throw std::runtime_error("Boot failed with exception: " + std::string(e.what())); - } - } else { - throw std::runtime_error("Boot timed out or failed."); - } - - } catch (const std::system_error & e) { - throw std::runtime_error( - "An exception occurred while trying to Boot driver " + std::string(e.what())); - } -} - -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/lynx_robot_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/lynx_robot_driver.cpp new file mode 100644 index 00000000..90abcfca --- /dev/null +++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/lynx_robot_driver.cpp @@ -0,0 +1,68 @@ +// 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 "husarion_ugv_hardware_interfaces/robot_system/robot_driver/lynx_robot_driver.hpp" + +#include +#include +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +void LynxRobotDriver::SendSpeedCommands(const std::vector & speeds) +{ + if (speeds.size() != 2) { + throw std::runtime_error( + "Invalid speeds vector size. Expected 2, got " + std::to_string(speeds.size())); + } + + const auto speed_left = this->GetCmdVelConverter().Convert(speeds.at(0)); + const auto speed_right = this->GetCmdVelConverter().Convert(speeds.at(1)); + + try { + drivers_.at(DriverNames::DEFAULT)->GetMotorDriver(MotorNames::LEFT)->SendCmdVel(speed_left); + drivers_.at(DriverNames::DEFAULT)->GetMotorDriver(MotorNames::RIGHT)->SendCmdVel(speed_right); + } catch (const std::runtime_error & e) { + throw std::runtime_error("Driver send Roboteq cmd failed: " + std::string(e.what())); + } + + if (drivers_.at(DriverNames::DEFAULT)->IsCANError()) { + throw std::runtime_error( + "CAN error detected on the Driver when trying to write speed commands."); + } +} + +void LynxRobotDriver::DefineDrivers() +{ + auto driver = std::make_shared( + canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(DriverNames::DEFAULT), + canopen_settings_.sdo_operation_timeout_ms); + + auto left_motor_driver = std::make_shared( + std::dynamic_pointer_cast(driver), MotorChannels::LEFT); + auto right_motor_driver = std::make_shared( + std::dynamic_pointer_cast(driver), MotorChannels::RIGHT); + + driver->AddMotorDriver(MotorNames::LEFT, left_motor_driver); + driver->AddMotorDriver(MotorNames::RIGHT, right_motor_driver); + + drivers_.emplace(DriverNames::DEFAULT, driver); +} + +} // namespace husarion_ugv_hardware_interfaces diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/panther_robot_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/panther_robot_driver.cpp new file mode 100644 index 00000000..9fe70960 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/panther_robot_driver.cpp @@ -0,0 +1,90 @@ +// 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 "husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp" + +#include +#include +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +void PantherRobotDriver::SendSpeedCommands(const std::vector & speeds) +{ + if (speeds.size() != 4) { + throw std::runtime_error( + "Invalid speeds vector size. Expected 4, got " + std::to_string(speeds.size())); + } + + const auto speed_fl = this->GetCmdVelConverter().Convert(speeds.at(0)); + const auto speed_fr = this->GetCmdVelConverter().Convert(speeds.at(1)); + const auto speed_rl = this->GetCmdVelConverter().Convert(speeds.at(2)); + const auto speed_rr = this->GetCmdVelConverter().Convert(speeds.at(3)); + + try { + drivers_.at(DriverNames::FRONT)->GetMotorDriver(MotorNames::LEFT)->SendCmdVel(speed_fl); + drivers_.at(DriverNames::FRONT)->GetMotorDriver(MotorNames::RIGHT)->SendCmdVel(speed_fr); + } catch (const std::runtime_error & e) { + throw std::runtime_error("Front driver send Roboteq cmd failed: " + std::string(e.what())); + } + try { + drivers_.at(DriverNames::REAR)->GetMotorDriver(MotorNames::LEFT)->SendCmdVel(speed_rl); + drivers_.at(DriverNames::REAR)->GetMotorDriver(MotorNames::RIGHT)->SendCmdVel(speed_rr); + } catch (const std::runtime_error & e) { + throw std::runtime_error("Rear driver send Roboteq cmd failed: " + std::string(e.what())); + } + + if (drivers_.at(DriverNames::FRONT)->IsCANError()) { + throw std::runtime_error( + "CAN error detected on the front driver when trying to write speed commands."); + } + if (drivers_.at(DriverNames::REAR)->IsCANError()) { + throw std::runtime_error( + "CAN error detected on the rear driver when trying to write speed commands."); + } +} + +void PantherRobotDriver::DefineDrivers() +{ + auto front_driver = std::make_shared( + canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(DriverNames::FRONT), + canopen_settings_.sdo_operation_timeout_ms); + auto rear_driver = std::make_shared( + canopen_manager_.GetMaster(), canopen_settings_.driver_can_ids.at(DriverNames::REAR), + canopen_settings_.sdo_operation_timeout_ms); + + auto fl_motor_driver = std::make_shared( + std::dynamic_pointer_cast(front_driver), MotorChannels::LEFT); + auto fr_motor_driver = std::make_shared( + std::dynamic_pointer_cast(front_driver), MotorChannels::RIGHT); + auto rl_motor_driver = std::make_shared( + std::dynamic_pointer_cast(rear_driver), MotorChannels::LEFT); + auto rr_motor_driver = std::make_shared( + std::dynamic_pointer_cast(rear_driver), MotorChannels::RIGHT); + + front_driver->AddMotorDriver(MotorNames::LEFT, fl_motor_driver); + front_driver->AddMotorDriver(MotorNames::RIGHT, fr_motor_driver); + rear_driver->AddMotorDriver(MotorNames::LEFT, rl_motor_driver); + rear_driver->AddMotorDriver(MotorNames::RIGHT, rr_motor_driver); + + drivers_.emplace(DriverNames::FRONT, front_driver); + drivers_.emplace(DriverNames::REAR, rear_driver); +} + +} // namespace husarion_ugv_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_data_converters.cpp similarity index 79% rename from panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_data_converters.cpp index d8d6991a..e68902e3 100644 --- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_data_converters.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_data_converters.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp" #include -#include "panther_hardware_interfaces/utils.hpp" -#include "panther_utils/common_utilities.hpp" +#include "husarion_ugv_hardware_interfaces/utils.hpp" +#include "husarion_ugv_utils/common_utilities.hpp" -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { RoboteqVelocityCommandConverter::RoboteqVelocityCommandConverter( @@ -202,16 +202,16 @@ std::map RuntimeError::GetErrorMap() const return error_map; } -void RoboteqData::SetMotorsStates( - const RoboteqMotorState & left_state, const RoboteqMotorState & right_state, +void DriverData::SetMotorsStates( + const MotorDriverState & channel_1_state, const MotorDriverState & channel_2_state, const bool data_timed_out) { - left_motor_state_.SetData(left_state); - right_motor_state_.SetData(right_state); + channel_1_motor_state_.SetData(channel_1_state); + channel_2_motor_state_.SetData(channel_2_state); motor_states_data_timed_out_ = data_timed_out; } -void RoboteqData::SetDriverState(const RoboteqDriverState & state, const bool data_timed_out) +void DriverData::SetDriverState(const DriverState & state, const bool data_timed_out) { driver_state_.SetTemperature(state.mcu_temp); driver_state_.SetHeatsinkTemperature(state.heatsink_temp); @@ -221,40 +221,62 @@ void RoboteqData::SetDriverState(const RoboteqDriverState & state, const bool da fault_flags_.SetData(state.fault_flags); script_flags_.SetData(state.script_flags); - left_runtime_error_.SetData(state.runtime_stat_flag_motor_2); - right_runtime_error_.SetData(state.runtime_stat_flag_motor_1); + channel_1_runtime_error_.SetData(state.runtime_stat_flag_channel_1); + channel_2_runtime_error_.SetData(state.runtime_stat_flag_channel_2); driver_state_data_timed_out_ = data_timed_out; } -std::string RoboteqData::GetFlagErrorLog() const +const MotorState & DriverData::GetMotorState(const std::uint8_t channel) const +{ + if (channel == RoboteqDriver::kChannel1) { + return channel_1_motor_state_; + } else if (channel == RoboteqDriver::kChannel2) { + return channel_2_motor_state_; + } + + throw std::runtime_error("Invalid channel number"); +} + +const RuntimeError & DriverData::GetRuntimeError(const std::uint8_t channel) const +{ + if (channel == RoboteqDriver::kChannel1) { + return channel_1_runtime_error_; + } else if (channel == RoboteqDriver::kChannel2) { + return channel_2_runtime_error_; + } + + throw std::runtime_error("Invalid channel number"); +} + +std::string DriverData::GetFlagErrorLog() const { return "Fault flags: " + fault_flags_.GetErrorLog() + "Script flags: " + script_flags_.GetErrorLog() + - "Left motor runtime flags: " + left_runtime_error_.GetErrorLog() + - "Right motor runtime flags: " + right_runtime_error_.GetErrorLog(); + "Channel 1 motor runtime flags: " + channel_1_runtime_error_.GetErrorLog() + + "Channel 2 motor runtime flags: " + channel_2_runtime_error_.GetErrorLog(); } -std::map RoboteqData::GetFlagErrorMap() const +std::map DriverData::GetFlagErrorMap() const { std::map flag_error_map; flag_error_map.merge(fault_flags_.GetErrorMap()); flag_error_map.merge(script_flags_.GetErrorMap()); - auto left_runtime_error_map = panther_utils::common_utilities::PrefixMapKeys( - left_runtime_error_.GetErrorMap(), "left_motor."); + auto channel_1_runtime_error_map = husarion_ugv_utils::common_utilities::PrefixMapKeys( + channel_1_runtime_error_.GetErrorMap(), "channel_1_motor."); - auto right_runtime_error_map = panther_utils::common_utilities::PrefixMapKeys( - right_runtime_error_.GetErrorMap(), "right_motor."); + auto channel_2_runtime_error_map = husarion_ugv_utils::common_utilities::PrefixMapKeys( + channel_2_runtime_error_.GetErrorMap(), "channel_2_motor."); - flag_error_map.merge(std::move(left_runtime_error_map)); - flag_error_map.merge(std::move(right_runtime_error_map)); + flag_error_map.merge(std::move(channel_1_runtime_error_map)); + flag_error_map.merge(std::move(channel_2_runtime_error_map)); return flag_error_map; } -std::map RoboteqData::GetErrorMap() const +std::map DriverData::GetErrorMap() const { std::map error_map; @@ -269,4 +291,4 @@ std::map RoboteqData::GetErrorMap() const return error_map; } -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_driver.cpp similarity index 66% rename from panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_driver.cpp index ec9bf523..1e05d1fb 100644 --- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_driver.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_driver.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp" #include #include @@ -24,34 +24,22 @@ #include #include -#include "panther_hardware_interfaces/utils.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" +#include "husarion_ugv_hardware_interfaces/utils.hpp" -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { -struct CANopenObject -{ - const std::uint16_t id; - const std::uint8_t subid; -}; - // All ids and sub ids were read directly from the eds file. Lely CANopen doesn't have the option // to parse them based on the ParameterName. Additionally between version v60 and v80 // ParameterName changed, for example: Cmd_ESTOP (old), Cmd_ESTOP Emergency Shutdown (new) As // parameter names changed, but ids stayed the same, it will be better to just use ids directly struct RoboteqCANObjects { - static constexpr CANopenObject cmd_1 = {0x2000, 1}; - static constexpr CANopenObject cmd_2 = {0x2000, 2}; - - static constexpr CANopenObject position_1 = {0x2104, 1}; - static constexpr CANopenObject position_2 = {0x2104, 2}; - - static constexpr CANopenObject velocity_1 = {0x2107, 1}; - static constexpr CANopenObject velocity_2 = {0x2107, 2}; - - static constexpr CANopenObject current_1 = {0x2100, 1}; - static constexpr CANopenObject current_2 = {0x2100, 2}; + static constexpr std::uint16_t cmd_id = 0x2000; + static constexpr std::uint16_t position_id = 0x2104; + static constexpr std::uint16_t velocity_id = 0x2107; + static constexpr std::uint16_t current_id = 0x2100; static constexpr CANopenObject flags = {0x2106, 7}; @@ -67,6 +55,44 @@ struct RoboteqCANObjects static constexpr CANopenObject turn_on_safety_stop = {0x202C, 0}; // Cmd_SFT }; +MotorDriverState RoboteqMotorDriver::ReadState() +{ + MotorDriverState state; + + if (auto driver = driver_.lock()) { + state.pos = driver->rpdo_mapped[RoboteqCANObjects::position_id][channel_]; + state.vel = driver->rpdo_mapped[RoboteqCANObjects::velocity_id][channel_]; + state.current = driver->rpdo_mapped[RoboteqCANObjects::current_id][channel_]; + state.pos_timestamp = driver->GetPositionTimestamp(channel_); + state.vel_current_timestamp = driver->GetSpeedCurrentTimestamp(channel_); + } + + return state; +} + +void RoboteqMotorDriver::SendCmdVel(const std::int32_t cmd) +{ + if (auto driver = driver_.lock()) { + driver->tpdo_mapped[RoboteqCANObjects::cmd_id][channel_] = cmd; + driver->tpdo_mapped[RoboteqCANObjects::cmd_id][channel_].WriteEvent(); + } +} + +void RoboteqMotorDriver::TurnOnSafetyStop() +{ + try { + if (auto driver = driver_.lock()) { + driver->SyncSDOWrite( + RoboteqCANObjects::turn_on_safety_stop.id, RoboteqCANObjects::turn_on_safety_stop.subid, + channel_); + } + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Error when trying to turn on safety stop on channel " + std::to_string(channel_) + ": " + + std::string(e.what())); + } +} + RoboteqDriver::RoboteqDriver( const std::shared_ptr & master, const std::uint8_t id, const std::chrono::milliseconds & sdo_operation_timeout_ms) @@ -77,6 +103,7 @@ RoboteqDriver::RoboteqDriver( std::future RoboteqDriver::Boot() { std::lock_guard lck(boot_mtx_); + boot_promise_ = std::promise(); std::future future = boot_promise_.get_future(); if (!LoopDriver::Boot()) { @@ -85,48 +112,15 @@ std::future RoboteqDriver::Boot() return future; } -RoboteqMotorsStates RoboteqDriver::ReadRoboteqMotorsStates() +DriverState RoboteqDriver::ReadState() { - RoboteqMotorsStates states; - - // already does locking when accessing rpdo - states.motor_1.pos = - rpdo_mapped[RoboteqCANObjects::position_1.id][RoboteqCANObjects::position_1.subid]; - states.motor_2.pos = - rpdo_mapped[RoboteqCANObjects::position_2.id][RoboteqCANObjects::position_2.subid]; - - states.motor_1.vel = - rpdo_mapped[RoboteqCANObjects::velocity_1.id][RoboteqCANObjects::velocity_1.subid]; - states.motor_2.vel = - rpdo_mapped[RoboteqCANObjects::velocity_2.id][RoboteqCANObjects::velocity_2.subid]; - - states.motor_1.current = - rpdo_mapped[RoboteqCANObjects::current_1.id][RoboteqCANObjects::current_1.subid]; - states.motor_2.current = - rpdo_mapped[RoboteqCANObjects::current_2.id][RoboteqCANObjects::current_2.subid]; - - { - std::lock_guard lck_p(position_timestamp_mtx_); - states.pos_timestamp = last_position_timestamp_; - } - - { - std::lock_guard lck_sc(speed_current_timestamp_mtx_); - states.vel_current_timestamp = last_speed_current_timestamp_; - } - - return states; -} - -RoboteqDriverState RoboteqDriver::ReadRoboteqDriverState() -{ - RoboteqDriverState state; + DriverState state; std::int32_t flags = static_cast( rpdo_mapped[RoboteqCANObjects::flags.id][RoboteqCANObjects::flags.subid]); state.fault_flags = GetByte(flags, 0); - state.runtime_stat_flag_motor_1 = GetByte(flags, 1); - state.runtime_stat_flag_motor_2 = GetByte(flags, 2); + state.runtime_stat_flag_channel_1 = GetByte(flags, 1); + state.runtime_stat_flag_channel_2 = GetByte(flags, 2); state.script_flags = GetByte(flags, 3); state.mcu_temp = rpdo_mapped[RoboteqCANObjects::mcu_temp.id][RoboteqCANObjects::mcu_temp.subid]; @@ -152,18 +146,7 @@ RoboteqDriverState RoboteqDriver::ReadRoboteqDriverState() return state; } -void RoboteqDriver::SendRoboteqCmd( - const std::int32_t cmd_channel_1, const std::int32_t cmd_channel_2) -{ - tpdo_mapped[RoboteqCANObjects::cmd_1.id][RoboteqCANObjects::cmd_1.subid] = cmd_channel_1; - tpdo_mapped[RoboteqCANObjects::cmd_2.id][RoboteqCANObjects::cmd_2.subid] = cmd_channel_2; - - // Both commands are in the TPDO 1, so write event for only one subid is triggered, as it will - // result in sending the whole TPDO 1 (both commands) - tpdo_mapped[RoboteqCANObjects::cmd_2.id][RoboteqCANObjects::cmd_2.subid].WriteEvent(); -} - -void RoboteqDriver::ResetRoboteqScript() +void RoboteqDriver::ResetScript() { try { SyncSDOWrite( @@ -193,26 +176,23 @@ void RoboteqDriver::TurnOffEStop() } } -void RoboteqDriver::TurnOnSafetyStopChannel1() +void RoboteqDriver::AddMotorDriver( + const std::string name, std::shared_ptr motor_driver) { - try { - SyncSDOWrite( - RoboteqCANObjects::turn_on_safety_stop.id, RoboteqCANObjects::turn_on_safety_stop.subid, 1); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Error when trying to turn on safety stop on channel 1: " + std::string(e.what())); + if (std::dynamic_pointer_cast(motor_driver) == nullptr) { + throw std::runtime_error("Motor driver is not of type RoboteqMotorDriver"); } + motor_drivers_.emplace(name, motor_driver); } -void RoboteqDriver::TurnOnSafetyStopChannel2() +std::shared_ptr RoboteqDriver::GetMotorDriver(const std::string & name) { - try { - SyncSDOWrite( - RoboteqCANObjects::turn_on_safety_stop.id, RoboteqCANObjects::turn_on_safety_stop.subid, 2); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Error when trying to turn on safety stop on channel 2: " + std::string(e.what())); + auto it = motor_drivers_.find(name); + if (it == motor_drivers_.end()) { + throw std::runtime_error("Motor driver with name '" + name + "' does not exist"); } + + return it->second; } template @@ -270,13 +250,18 @@ void RoboteqDriver::OnBoot( void RoboteqDriver::OnRpdoWrite(const std::uint16_t idx, const std::uint8_t subidx) noexcept { - if (idx == RoboteqCANObjects::position_1.id && subidx == RoboteqCANObjects::position_1.subid) { + if (idx == RoboteqCANObjects::position_id) { + if (subidx != kChannel1 && subidx != kChannel2) { + return; + } std::lock_guard lck(position_timestamp_mtx_); - clock_gettime(CLOCK_MONOTONIC, &last_position_timestamp_); - } else if ( - idx == RoboteqCANObjects::velocity_1.id && subidx == RoboteqCANObjects::velocity_1.subid) { + clock_gettime(CLOCK_MONOTONIC, &last_position_timestamps_.at(subidx)); + } else if (idx == RoboteqCANObjects::velocity_id) { + if (subidx != kChannel1 && subidx != kChannel2) { + return; + } std::lock_guard lck(speed_current_timestamp_mtx_); - clock_gettime(CLOCK_MONOTONIC, &last_speed_current_timestamp_); + clock_gettime(CLOCK_MONOTONIC, &last_speed_current_timestamps_.at(subidx)); } else if (idx == RoboteqCANObjects::flags.id && subidx == RoboteqCANObjects::flags.subid) { std::lock_guard lck(flags_current_timestamp_mtx_); clock_gettime(CLOCK_MONOTONIC, &flags_current_timestamp_); @@ -288,4 +273,4 @@ void RoboteqDriver::OnRpdoWrite(const std::uint16_t idx, const std::uint8_t subi } } -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_error_filter.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_error_filter.cpp similarity index 93% rename from panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_error_filter.cpp rename to husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_error_filter.cpp index c0092af9..81063c31 100644 --- a/panther_hardware_interfaces/src/panther_system/motors_controller/roboteq_error_filter.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_error_filter.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_error_filter.hpp" #include #include #include -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { void ErrorFilter::UpdateError(const bool current_error) @@ -91,4 +91,4 @@ void RoboteqErrorFilter::ClearErrorsIfFlagSet() } } -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_robot_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_robot_driver.cpp new file mode 100644 index 00000000..18a61d91 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_robot_driver.cpp @@ -0,0 +1,260 @@ +// 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 "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "lely/util/chrono.hpp" + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +RoboteqRobotDriver::RoboteqRobotDriver( + const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings, + const std::chrono::milliseconds activate_wait_time) +: canopen_settings_(canopen_settings), + drivetrain_settings_(drivetrain_settings), + canopen_manager_(canopen_settings), + roboteq_vel_cmd_converter_(drivetrain_settings), + pdo_motor_states_timeout_ms_(canopen_settings.pdo_motor_states_timeout_ms), + pdo_driver_state_timeout_ms_(canopen_settings.pdo_driver_state_timeout_ms), + activate_wait_time_(activate_wait_time) +{ +} + +void RoboteqRobotDriver::Initialize() +{ + if (initialized_) { + return; + } + + try { + canopen_manager_.Initialize(); + DefineDrivers(); + for (auto & [name, driver] : drivers_) { + data_.emplace(name, DriverData(drivetrain_settings_)); + } + + canopen_manager_.Activate(); + BootDrivers(); + + } catch (const std::runtime_error & e) { + throw std::runtime_error("Failed to initialize robot driver: " + std::string(e.what())); + } + + initialized_ = true; +} + +void RoboteqRobotDriver::Deinitialize() +{ + canopen_manager_.Deinitialize(); + initialized_ = false; +} + +void RoboteqRobotDriver::Activate() +{ + // Activation procedure - it is necessary to first reset scripts, wait for a bit (1 second) + // and then send 0 commands for some time (also 1 second) + + for (auto & [name, driver] : drivers_) { + try { + driver->ResetScript(); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Reset Roboteq script exception on " + name + " driver : " + std::string(e.what())); + } + } + + std::this_thread::sleep_for(activate_wait_time_); + + for (auto & [name, driver] : drivers_) { + try { + driver->GetMotorDriver(MotorNames::LEFT)->SendCmdVel(0); + driver->GetMotorDriver(MotorNames::RIGHT)->SendCmdVel(0); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Send 0 command exception on " + name + "driver : " + std::string(e.what())); + } + } + + std::this_thread::sleep_for(activate_wait_time_); +} + +void RoboteqRobotDriver::UpdateCommunicationState() +{ + for (auto & [name, driver] : drivers_) { + data_.at(name).SetCANError(driver->IsCANError()); + data_.at(name).SetHeartbeatTimeout(driver->IsHeartbeatTimeout()); + } +} + +void RoboteqRobotDriver::UpdateMotorsState() +{ + timespec current_time; + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + + for (auto & [name, driver] : drivers_) { + const auto left_state = driver->GetMotorDriver(MotorNames::LEFT)->ReadState(); + const auto right_state = driver->GetMotorDriver(MotorNames::RIGHT)->ReadState(); + + SetMotorsStates(data_.at(name), left_state, right_state, current_time); + } + + UpdateCommunicationState(); + + if (std::any_of( + data_.begin(), data_.end(), [](const auto & data) { return data.second.IsCANError(); })) { + throw std::runtime_error("CAN error."); + } + + if (std::any_of(data_.begin(), data_.end(), [](const auto & data) { + return data.second.IsHeartbeatTimeout(); + })) { + throw std::runtime_error("Motor controller heartbeat timeout."); + } +} + +void RoboteqRobotDriver::UpdateDriversState() +{ + timespec current_time; + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + + for (auto & [name, driver] : drivers_) { + SetDriverState(data_.at(name), driver->ReadState(), current_time); + } + + UpdateCommunicationState(); + + if (std::any_of( + data_.begin(), data_.end(), [](const auto & data) { return data.second.IsCANError(); })) { + throw std::runtime_error("CAN error."); + } + + if (std::any_of(data_.begin(), data_.end(), [](const auto & data) { + return data.second.IsHeartbeatTimeout(); + })) { + throw std::runtime_error("Motor controller heartbeat timeout."); + } +} + +const DriverData & RoboteqRobotDriver::GetData(const std::string & name) +{ + if (data_.find(name) == data_.end()) { + throw std::runtime_error("Data with name '" + name + "' does not exist."); + } + + return data_.at(name); +} + +void RoboteqRobotDriver::TurnOnEStop() +{ + for (auto & [name, driver] : drivers_) { + try { + driver->TurnOnEStop(); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to turn on E-stop on " + name + " driver: " + std::string(e.what())); + } + } +} + +void RoboteqRobotDriver::TurnOffEStop() +{ + for (auto & [name, driver] : drivers_) { + try { + driver->TurnOffEStop(); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to turn off E-stop on " + name + " driver: " + std::string(e.what())); + } + } +} + +bool RoboteqRobotDriver::CommunicationError() +{ + return std::any_of(data_.begin(), data_.end(), [](const auto & data) { + return data.second.IsCANError() || data.second.IsHeartbeatTimeout(); + }); +} + +void RoboteqRobotDriver::SetMotorsStates( + DriverData & data, const MotorDriverState & left_state, const MotorDriverState & right_state, + const timespec & current_time) +{ + const bool data_timed_out = + DataTimeout(current_time, left_state.pos_timestamp, pdo_motor_states_timeout_ms_) || + DataTimeout(current_time, left_state.vel_current_timestamp, pdo_motor_states_timeout_ms_) || + DataTimeout(current_time, right_state.pos_timestamp, pdo_motor_states_timeout_ms_) || + DataTimeout(current_time, right_state.vel_current_timestamp, pdo_motor_states_timeout_ms_); + + // Channel 1 - right, Channel 2 - left + data.SetMotorsStates(right_state, left_state, data_timed_out); +} + +void RoboteqRobotDriver::SetDriverState( + DriverData & data, const DriverState & state, const timespec & current_time) +{ + const bool data_timed_out = + DataTimeout(current_time, state.flags_current_timestamp, pdo_driver_state_timeout_ms_) || + DataTimeout(current_time, state.voltages_temps_timestamp, pdo_driver_state_timeout_ms_); + + data.SetDriverState(state, data_timed_out); +} + +bool RoboteqRobotDriver::DataTimeout( + const timespec & current_time, const timespec & data_timestamp, + const std::chrono::milliseconds & timeout) +{ + return lely::util::from_timespec(current_time) - lely::util::from_timespec(data_timestamp) > + timeout; +} + +void RoboteqRobotDriver::BootDrivers() +{ + for (auto & [name, driver] : drivers_) { + try { + auto driver_future = driver->Boot(); + auto driver_status = driver_future.wait_for(std::chrono::seconds(5)); + + if (driver_status == std::future_status::ready) { + try { + driver_future.get(); + } catch (const std::exception & e) { + throw std::runtime_error( + "Boot for " + name + " driver failed with exception: " + std::string(e.what())); + } + } else { + throw std::runtime_error("Boot for " + name + " driver timed out or failed."); + } + + } catch (const std::system_error & e) { + throw std::runtime_error( + "An exception occurred while trying to Boot " + name + " driver " + std::string(e.what())); + } + } +} + +} // namespace husarion_ugv_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_e_stop.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/system_e_stop.cpp similarity index 55% rename from panther_hardware_interfaces/src/panther_system/panther_system_e_stop.cpp rename to husarion_ugv_hardware_interfaces/src/robot_system/system_e_stop.cpp index 9a776b40..969b3956 100644 --- a/panther_hardware_interfaces/src/panther_system/panther_system_e_stop.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/system_e_stop.cpp @@ -17,12 +17,12 @@ #include #include -#include "panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp" -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { -bool EStopPTH12X::ReadEStopState() +bool EStop::ReadEStopState() { if (e_stop_manipulation_mtx_.try_lock()) { std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); @@ -39,7 +39,7 @@ bool EStopPTH12X::ReadEStopState() return e_stop_triggered_; } -void EStopPTH12X::TriggerEStop() +void EStop::TriggerEStop() { gpio_controller_->InterruptEStopReset(); @@ -53,7 +53,7 @@ void EStopPTH12X::TriggerEStop() e_stop_triggered_ = true; } -void EStopPTH12X::ResetEStop() +void EStop::ResetEStop() { if (e_stop_manipulation_mtx_.try_lock()) { std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); @@ -83,61 +83,4 @@ void EStopPTH12X::ResetEStop() } } -bool EStopPTH10X::ReadEStopState() -{ - if (e_stop_manipulation_mtx_.try_lock()) { - std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); - const bool motors_on = gpio_controller_->IsPinActive(GPIOPin::STAGE2_INPUT); - const bool driver_error = roboteq_error_filter_->IsError(); - - if ((driver_error || !motors_on) && !e_stop_triggered_) { - TriggerEStop(); - } - } - - return e_stop_triggered_; -} - -void EStopPTH10X::TriggerEStop() -{ - std::lock_guard e_stop_lck(e_stop_manipulation_mtx_); - std::lock_guard lck_g(*motor_controller_write_mtx_); - - try { - motors_controller_->TurnOnSafetyStop(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Error when trying to set safety stop using CAN command: " + std::string(e.what())); - } - - e_stop_triggered_ = true; -} - -void EStopPTH10X::ResetEStop() -{ - if (e_stop_manipulation_mtx_.try_lock()) { - std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); - - if (!ZeroVelocityCheck()) { - throw std::runtime_error( - "Can't reset E-stop - last velocity commands are different than zero. Make sure that your " - "controller sends zero commands before trying to reset E-stop."); - } - - if (!gpio_controller_->IsPinActive(GPIOPin::STAGE2_INPUT)) { - throw std::runtime_error("Can't reset E-stop - motors are not powered."); - } - - if (roboteq_error_filter_->IsError()) { - throw std::runtime_error("Can't reset E-stop - motor controller is in an error state."); - } - - roboteq_error_filter_->SetClearErrorsFlag(); - - e_stop_triggered_ = false; - } else { - std::fprintf(stdout, "E-stop trigger operation is in progress, skipping reset."); - } -} - -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/system_ros_interface.cpp similarity index 65% rename from panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp rename to husarion_ugv_hardware_interfaces/src/robot_system/system_ros_interface.cpp index 7bab66df..560789d2 100644 --- a/panther_hardware_interfaces/src/panther_system/panther_system_ros_interface.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/system_ros_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/panther_system/panther_system_ros_interface.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp" #include #include @@ -22,9 +22,9 @@ #include "rclcpp/rclcpp.hpp" #include "realtime_tools/realtime_publisher.h" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp" -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { template class ROSServiceWrapper>; @@ -52,7 +52,7 @@ void ROSServiceWrapper::CallbackWrapper( response->message = err.what(); RCLCPP_WARN_STREAM( - rclcpp::get_logger("PantherSystem"), + rclcpp::get_logger("UGVSystem"), "An exception occurred while handling the request: " << err.what()); } } @@ -71,20 +71,21 @@ void ROSServiceWrapper>::ProccessC callback_(); } -PantherSystemRosInterface::PantherSystemRosInterface( +SystemROSInterface::SystemROSInterface( const std::string & node_name, const rclcpp::NodeOptions & node_options) : node_(rclcpp::Node::make_shared(node_name, node_options)), diagnostic_updater_(node_) { - RCLCPP_INFO(rclcpp::get_logger("PantherSystem"), "Constructing node."); + RCLCPP_INFO(rclcpp::get_logger("UGVSystem"), "Constructing node."); executor_ = std::make_unique(); executor_->add_node(node_); executor_thread_ = std::thread([this]() { executor_->spin(); }); - driver_state_publisher_ = node_->create_publisher("~/motor_controllers_state", 5); + driver_state_publisher_ = node_->create_publisher("~/robot_driver_state", 5); realtime_driver_state_publisher_ = - std::make_unique>(driver_state_publisher_); + std::make_unique>( + driver_state_publisher_); io_state_publisher_ = node_->create_publisher( "~/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -96,12 +97,12 @@ PantherSystemRosInterface::PantherSystemRosInterface( realtime_e_stop_state_publisher_ = std::make_unique>(e_stop_state_publisher_); - diagnostic_updater_.setHardwareID("Panther System"); + diagnostic_updater_.setHardwareID("UGV System"); - RCLCPP_INFO(rclcpp::get_logger("PantherSystem"), "Node constructed successfully."); + RCLCPP_INFO(rclcpp::get_logger("UGVSystem"), "Node constructed successfully."); } -PantherSystemRosInterface::~PantherSystemRosInterface() +SystemROSInterface::~SystemROSInterface() { if (executor_) { executor_->cancel(); @@ -125,41 +126,34 @@ PantherSystemRosInterface::~PantherSystemRosInterface() node_.reset(); } -void PantherSystemRosInterface::UpdateMsgErrorFlags( - const RoboteqData & front, const RoboteqData & rear) +void SystemROSInterface::UpdateMsgErrorFlags(const std::string & name, const DriverData & data) { auto & driver_state = realtime_driver_state_publisher_->msg_; + auto & driver_state_named = GetDriverStateByName(driver_state, name); driver_state.header.stamp = node_->get_clock()->now(); - driver_state.front.fault_flag = front.GetFaultFlag().GetMessage(); - driver_state.front.script_flag = front.GetScriptFlag().GetMessage(); - driver_state.front.left_motor_runtime_error = front.GetLeftRuntimeError().GetMessage(); - driver_state.front.right_motor_runtime_error = front.GetRightRuntimeError().GetMessage(); - - driver_state.rear.fault_flag = rear.GetFaultFlag().GetMessage(); - driver_state.rear.script_flag = rear.GetScriptFlag().GetMessage(); - driver_state.rear.left_motor_runtime_error = rear.GetLeftRuntimeError().GetMessage(); - driver_state.rear.right_motor_runtime_error = rear.GetRightRuntimeError().GetMessage(); + driver_state_named.state.fault_flag = data.GetFaultFlag().GetMessage(); + driver_state_named.state.script_flag = data.GetScriptFlag().GetMessage(); + driver_state_named.state.channel_1_motor_runtime_error = + data.GetRuntimeError(RoboteqDriver::kChannel1).GetMessage(); + driver_state_named.state.channel_2_motor_runtime_error = + data.GetRuntimeError(RoboteqDriver::kChannel2).GetMessage(); } -void PantherSystemRosInterface::UpdateMsgDriversStates( - const DriverState & front, const DriverState & rear) +void SystemROSInterface::UpdateMsgDriversStates( + const std::string & name, const RoboteqDriverState & state) { auto & driver_state = realtime_driver_state_publisher_->msg_; + auto & driver_state_named = GetDriverStateByName(driver_state, name); - driver_state.front.voltage = front.GetVoltage(); - driver_state.front.current = front.GetCurrent(); - driver_state.front.temperature = front.GetTemperature(); - driver_state.front.heatsink_temperature = front.GetHeatsinkTemperature(); - - driver_state.rear.voltage = rear.GetVoltage(); - driver_state.rear.current = rear.GetCurrent(); - driver_state.rear.temperature = rear.GetTemperature(); - driver_state.rear.heatsink_temperature = rear.GetHeatsinkTemperature(); + driver_state_named.state.voltage = state.GetVoltage(); + driver_state_named.state.current = state.GetCurrent(); + driver_state_named.state.temperature = state.GetTemperature(); + driver_state_named.state.heatsink_temperature = state.GetHeatsinkTemperature(); } -void PantherSystemRosInterface::UpdateMsgErrors(const CANErrors & can_errors) +void SystemROSInterface::UpdateMsgErrors(const CANErrors & can_errors) { auto & driver_state = realtime_driver_state_publisher_->msg_; @@ -168,20 +162,19 @@ void PantherSystemRosInterface::UpdateMsgErrors(const CANErrors & can_errors) driver_state.read_pdo_motor_states_error = can_errors.read_pdo_motor_states_error; driver_state.read_pdo_driver_state_error = can_errors.read_pdo_driver_state_error; - driver_state.front.motor_states_data_timed_out = can_errors.front_motor_states_data_timed_out; - driver_state.rear.motor_states_data_timed_out = can_errors.rear_motor_states_data_timed_out; - - driver_state.front.driver_state_data_timed_out = can_errors.front_driver_state_data_timed_out; - driver_state.rear.driver_state_data_timed_out = can_errors.rear_driver_state_data_timed_out; + for (const auto & [name, driver_errors] : can_errors.driver_errors) { + auto & driver_state_named = GetDriverStateByName(driver_state, name); - driver_state.front.can_error = can_errors.front_can_error; - driver_state.rear.can_error = can_errors.rear_can_error; - - driver_state.front.heartbeat_timeout = can_errors.front_heartbeat_timeout; - driver_state.rear.heartbeat_timeout = can_errors.rear_heartbeat_timeout; + driver_state_named.state.motor_states_data_timed_out = + driver_errors.motor_states_data_timed_out; + driver_state_named.state.driver_state_data_timed_out = + driver_errors.driver_state_data_timed_out; + driver_state_named.state.can_error = driver_errors.can_error; + driver_state_named.state.heartbeat_timeout = driver_errors.heartbeat_timeout; + } } -void PantherSystemRosInterface::PublishEStopStateMsg(const bool e_stop) +void SystemROSInterface::PublishEStopStateMsg(const bool e_stop) { realtime_e_stop_state_publisher_->msg_.data = e_stop; if (realtime_e_stop_state_publisher_->trylock()) { @@ -189,21 +182,21 @@ void PantherSystemRosInterface::PublishEStopStateMsg(const bool e_stop) } } -void PantherSystemRosInterface::PublishEStopStateIfChanged(const bool e_stop) +void SystemROSInterface::PublishEStopStateIfChanged(const bool e_stop) { if (realtime_e_stop_state_publisher_->msg_.data != e_stop) { PublishEStopStateMsg(e_stop); } } -void PantherSystemRosInterface::PublishDriverState() +void SystemROSInterface::PublishRobotDriverState() { if (realtime_driver_state_publisher_->trylock()) { realtime_driver_state_publisher_->unlockAndPublish(); } } -void PantherSystemRosInterface::InitializeAndPublishIOStateMsg( +void SystemROSInterface::InitializeAndPublishIOStateMsg( const std::unordered_map & io_state) { for (const auto & [pin, pin_value] : io_state) { @@ -215,7 +208,7 @@ void PantherSystemRosInterface::InitializeAndPublishIOStateMsg( } } -void PantherSystemRosInterface::PublishIOState(const GPIOInfo & gpio_info) +void SystemROSInterface::PublishIOState(const GPIOInfo & gpio_info) { const bool pin_value = (gpio_info.value == gpiod::line::value::ACTIVE); @@ -228,7 +221,7 @@ void PantherSystemRosInterface::PublishIOState(const GPIOInfo & gpio_info) } } -bool PantherSystemRosInterface::UpdateIOStateMsg(const GPIOPin pin, const bool pin_value) +bool SystemROSInterface::UpdateIOStateMsg(const GPIOPin pin, const bool pin_value) { auto & io_state_msg = realtime_io_state_publisher_->msg_; @@ -265,7 +258,7 @@ bool PantherSystemRosInterface::UpdateIOStateMsg(const GPIOPin pin, const bool p return true; } -rclcpp::CallbackGroup::SharedPtr PantherSystemRosInterface::GetOrCreateNodeCallbackGroup( +rclcpp::CallbackGroup::SharedPtr SystemROSInterface::GetOrCreateNodeCallbackGroup( const unsigned group_id, rclcpp::CallbackGroupType callback_group_type) { if (group_id == 0) { @@ -290,4 +283,23 @@ rclcpp::CallbackGroup::SharedPtr PantherSystemRosInterface::GetOrCreateNodeCallb return callback_group; } -} // namespace panther_hardware_interfaces +DriverStateNamedMsg & SystemROSInterface::GetDriverStateByName( + RobotDriverStateMsg & robot_driver_state, const std::string & name) +{ + auto & driver_states = robot_driver_state.driver_states; + + auto it = std::find_if( + driver_states.begin(), driver_states.end(), + [&name](const DriverStateNamedMsg & msg) { return msg.name == name; }); + + if (it == driver_states.end()) { + DriverStateNamedMsg driver_state_named; + driver_state_named.name = name; + driver_states.push_back(driver_state_named); + it = driver_states.end() - 1; + } + + return *it; +} + +} // namespace husarion_ugv_hardware_interfaces diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp new file mode 100644 index 00000000..1915cf6e --- /dev/null +++ b/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp @@ -0,0 +1,550 @@ +// 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 "husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/panther_robot_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/system_ros_interface.hpp" +#include "husarion_ugv_hardware_interfaces/utils.hpp" + +#include "husarion_ugv_utils/common_utilities.hpp" +#include "husarion_ugv_utils/diagnostics.hpp" + +namespace husarion_ugv_hardware_interfaces +{ + +CallbackReturn UGVSystem::on_init(const hardware_interface::HardwareInfo & hardware_info) +{ + if (hardware_interface::SystemInterface::on_init(hardware_info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + try { + CheckJointSize(); + SortAndCheckJointNames(); + SetInitialValues(); + CheckInterfaces(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger_, "An exception occurred while initializing: " << e.what()); + return CallbackReturn::ERROR; + } + + try { + ReadDrivetrainSettings(); + ReadCANopenSettings(); + ReadInitializationActivationAttempts(); + ReadParametersAndCreateRoboteqErrorFilter(); + ReadDriverStatesUpdateFrequency(); + } catch (const std::invalid_argument & e) { + RCLCPP_ERROR_STREAM( + logger_, "An exception occurred while reading the parameters: " << e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_configure(const rclcpp_lifecycle::State &) +{ + try { + ConfigureGPIOController(); + ConfigureRobotDriver(); + ConfigureEStop(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM( + logger_, "Failed to initialize GPIO, Motors, or E-Stop controllers. Error: " << e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_cleanup(const rclcpp_lifecycle::State &) +{ + robot_driver_->Deinitialize(); + robot_driver_.reset(); + + gpio_controller_.reset(); + e_stop_.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_activate(const rclcpp_lifecycle::State &) +{ + std::fill(hw_commands_velocities_.begin(), hw_commands_velocities_.end(), 0.0); + std::fill(hw_states_positions_.begin(), hw_states_positions_.end(), 0.0); + std::fill(hw_states_velocities_.begin(), hw_states_velocities_.end(), 0.0); + std::fill(hw_states_efforts_.begin(), hw_states_efforts_.end(), 0.0); + + if (!OperationWithAttempts( + std::bind(&RobotDriverInterface::Activate, robot_driver_), + max_roboteq_activation_attempts_)) { + RCLCPP_ERROR_STREAM( + logger_, "Failed to activate UGV System: Couldn't activate RobotDriver in " + << max_roboteq_activation_attempts_ << " attempts."); + return CallbackReturn::ERROR; + } + + system_ros_interface_ = std::make_unique("hardware_controller"); + + system_ros_interface_->AddService>( + "~/fan_enable", + std::bind(&GPIOControllerInterface::FanEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/aux_power_enable", + std::bind(&GPIOControllerInterface::AUXPowerEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/digital_power_enable", + std::bind( + &GPIOControllerInterface::DigitalPowerEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/charger_enable", + std::bind(&GPIOControllerInterface::ChargerEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/led_control_enable", + std::bind(&GPIOControllerInterface::LEDControlEnable, gpio_controller_, std::placeholders::_1)); + system_ros_interface_->AddService>( + "~/motor_power_enable", std::bind(&UGVSystem::MotorsPowerEnable, this, std::placeholders::_1)); + + system_ros_interface_->AddService>( + "~/e_stop_trigger", std::bind(&EStopInterface::TriggerEStop, e_stop_), 1, + rclcpp::CallbackGroupType::MutuallyExclusive); + + auto e_stop_reset_qos = rmw_qos_profile_services_default; + e_stop_reset_qos.depth = 1; + system_ros_interface_->AddService>( + "~/e_stop_reset", std::bind(&EStopInterface::ResetEStop, e_stop_), 2, + rclcpp::CallbackGroupType::MutuallyExclusive, e_stop_reset_qos); + + system_ros_interface_->AddDiagnosticTask( + std::string("system errors"), this, &UGVSystem::DiagnoseErrors); + + system_ros_interface_->AddDiagnosticTask( + std::string("system status"), this, &UGVSystem::DiagnoseStatus); + + gpio_controller_->RegisterGPIOEventCallback( + [this](const auto & state) { system_ros_interface_->PublishIOState(state); }); + + const auto io_state = gpio_controller_->QueryControlInterfaceIOStates(); + system_ros_interface_->InitializeAndPublishIOStateMsg(io_state); + + system_ros_interface_->PublishEStopStateMsg(e_stop_->ReadEStopState()); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_deactivate(const rclcpp_lifecycle::State &) +{ + try { + e_stop_->TriggerEStop(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger_, "Deactivation failed: " << e.what()); + return CallbackReturn::ERROR; + } + + system_ros_interface_.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_shutdown(const rclcpp_lifecycle::State &) +{ + try { + e_stop_->TriggerEStop(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger_, "Shutdown failed: " << e.what()); + return CallbackReturn::ERROR; + } + + robot_driver_->Deinitialize(); + robot_driver_.reset(); + + gpio_controller_.reset(); + + system_ros_interface_.reset(); + e_stop_.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn UGVSystem::on_error(const rclcpp_lifecycle::State &) +{ + try { + e_stop_->TriggerEStop(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger_, "Handling error failed: " << e.what()); + return CallbackReturn::ERROR; + } + + if (system_ros_interface_) { + system_ros_interface_->BroadcastOnDiagnosticTasks( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "An error has occurred during a node state transition."); + + system_ros_interface_.reset(); + } + + robot_driver_->Deinitialize(); + robot_driver_.reset(); + + gpio_controller_.reset(); + e_stop_.reset(); + + return CallbackReturn::SUCCESS; +} + +std::vector UGVSystem::export_state_interfaces() +{ + std::vector state_interfaces; + for (std::size_t i = 0; i < joint_size_; i++) { + state_interfaces.emplace_back(StateInterface( + joints_names_sorted_[i], hardware_interface::HW_IF_POSITION, &hw_states_positions_[i])); + state_interfaces.emplace_back(StateInterface( + joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i])); + state_interfaces.emplace_back(StateInterface( + joints_names_sorted_[i], hardware_interface::HW_IF_EFFORT, &hw_states_efforts_[i])); + } + + return state_interfaces; +} + +std::vector UGVSystem::export_command_interfaces() +{ + std::vector command_interfaces; + for (std::size_t i = 0; i < joint_size_; i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i])); + } + + return command_interfaces; +} + +return_type UGVSystem::read(const rclcpp::Time & time, const rclcpp::Duration & /* period */) +{ + UpdateMotorsState(); + + if (time >= next_driver_state_update_time_) { + UpdateDriverState(); + UpdateDriverStateMsg(); + system_ros_interface_->PublishRobotDriverState(); + next_driver_state_update_time_ = time + driver_states_update_period_; + } + + UpdateEStopState(); + + return return_type::OK; +} + +return_type UGVSystem::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +{ + const bool e_stop = e_stop_->ReadEStopState(); + + if (!e_stop) { + HandleRobotDriverWriteOperation([this] { + const auto speed_cmds = GetSpeedCommands(); + robot_driver_->SendSpeedCommands(speed_cmds); + }); + } + + return return_type::OK; +} + +void UGVSystem::CheckJointSize() const +{ + if (info_.joints.size() != joint_size_) { + throw std::runtime_error( + "Wrong number of joints defined: " + std::to_string(info_.joints.size()) + ", " + + std::to_string(joint_size_) + " expected."); + } +} + +void UGVSystem::SortAndCheckJointNames() +{ + // Sort joints names - later hw_states and hw_commands are accessed by static indexes, so it + // is necessary to make sure that joints are in specific order and order of definitions in URDF + // doesn't matter + for (std::size_t i = 0; i < joint_size_; i++) { + std::size_t match_count = 0; + + for (std::size_t j = 0; j < joint_size_; j++) { + if (CheckIfJointNameContainValidSequence(info_.joints[j].name, joint_order_[i])) { + joints_names_sorted_[i] = info_.joints[j].name; + ++match_count; + } + } + + if (match_count != 1) { + throw std::runtime_error( + "There should be exactly one joint containing " + joint_order_[i] + ", " + + std::to_string(match_count) + " found."); + } + } +} + +void UGVSystem::SetInitialValues() +{ + // It isn't safe to set command to NaN - sometimes it could be interpreted as Inf (although it + // shouldn't). In case of velocity, I think that setting the initial value to 0.0 is the best + // option. + hw_commands_velocities_.resize(joint_size_, 0.0); + + hw_states_positions_.resize(joint_size_, std::numeric_limits::quiet_NaN()); + hw_states_velocities_.resize(joint_size_, std::numeric_limits::quiet_NaN()); + hw_states_efforts_.resize(joint_size_, std::numeric_limits::quiet_NaN()); +} + +void UGVSystem::CheckInterfaces() const +{ + for (const hardware_interface::ComponentInfo & joint : info_.joints) { + // Commands + if (joint.command_interfaces.size() != 1) { + throw std::runtime_error( + "Joint " + joint.name + " has " + std::to_string(joint.command_interfaces.size()) + + " command interfaces. 1 expected."); + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { + throw std::runtime_error( + "Joint " + joint.name + " has " + joint.command_interfaces[0].name + + " command interface. " + hardware_interface::HW_IF_VELOCITY + " expected."); + } + + // States + if (joint.state_interfaces.size() != 3) { + throw std::runtime_error( + "Joint " + joint.name + " has " + std::to_string(joint.state_interfaces.size()) + + " state " + (joint.state_interfaces.size() == 1 ? "interface." : "interfaces.") + + " 3 expected."); + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + throw std::runtime_error( + "Joint " + joint.name + " has " + joint.state_interfaces[0].name + + " as first state interface. " + hardware_interface::HW_IF_POSITION + " expected."); + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { + throw std::runtime_error( + "Joint " + joint.name + " has " + joint.state_interfaces[1].name + + " as second state interface. " + hardware_interface::HW_IF_VELOCITY + " expected."); + } + + if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { + throw std::runtime_error( + "Joint " + joint.name + " has " + joint.state_interfaces[2].name + + " as third state interface. " + hardware_interface::HW_IF_EFFORT + " expected."); + } + } +} + +void UGVSystem::ReadDrivetrainSettings() +{ + drivetrain_settings_.motor_torque_constant = + std::stof(info_.hardware_parameters["motor_torque_constant"]); + drivetrain_settings_.gear_ratio = std::stof(info_.hardware_parameters["gear_ratio"]); + drivetrain_settings_.gearbox_efficiency = + std::stof(info_.hardware_parameters["gearbox_efficiency"]); + drivetrain_settings_.encoder_resolution = + std::stof(info_.hardware_parameters["encoder_resolution"]); + drivetrain_settings_.max_rpm_motor_speed = + std::stof(info_.hardware_parameters["max_rpm_motor_speed"]); +} + +void UGVSystem::ReadCANopenSettings() +{ + canopen_settings_.can_interface_name = info_.hardware_parameters["can_interface_name"]; + canopen_settings_.master_can_id = std::stoi(info_.hardware_parameters["master_can_id"]); + canopen_settings_.pdo_motor_states_timeout_ms = + std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_motor_states_timeout_ms"])); + canopen_settings_.pdo_driver_state_timeout_ms = + std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_driver_state_timeout_ms"])); + canopen_settings_.sdo_operation_timeout_ms = + std::chrono::milliseconds(std::stoi(info_.hardware_parameters["sdo_operation_timeout_ms"])); + + ReadCANopenSettingsDriverCANIDs(); +} + +void UGVSystem::ReadInitializationActivationAttempts() +{ + max_roboteq_initialization_attempts_ = + std::stoi(info_.hardware_parameters["max_roboteq_initialization_attempts"]); + max_roboteq_activation_attempts_ = + std::stoi(info_.hardware_parameters["max_roboteq_activation_attempts"]); +} + +void UGVSystem::ReadParametersAndCreateRoboteqErrorFilter() +{ + const unsigned max_write_pdo_cmds_errors_count = + std::stoi(info_.hardware_parameters["max_write_pdo_cmds_errors_count"]); + const unsigned max_read_pdo_motor_states_errors_count = + std::stoi(info_.hardware_parameters["max_read_pdo_motor_states_errors_count"]); + const unsigned max_read_pdo_driver_state_errors_count = + std::stoi(info_.hardware_parameters["max_read_pdo_driver_state_errors_count"]); + + roboteq_error_filter_ = std::make_shared( + max_write_pdo_cmds_errors_count, max_read_pdo_motor_states_errors_count, + max_read_pdo_driver_state_errors_count, 1); +} + +void UGVSystem::ReadDriverStatesUpdateFrequency() +{ + const float driver_states_update_frequency = + std::stof(info_.hardware_parameters["driver_states_update_frequency"]); + driver_states_update_period_ = + rclcpp::Duration::from_seconds(1.0f / driver_states_update_frequency); +} + +void UGVSystem::ConfigureGPIOController() +{ + gpio_controller_ = GPIOControllerFactory::CreateGPIOController(); + gpio_controller_->Start(); + + RCLCPP_INFO(logger_, "Successfully configured GPIO controller."); +} + +void UGVSystem::ConfigureRobotDriver() +{ + robot_driver_write_mtx_ = std::make_shared(); + DefineRobotDriver(); + + if (!OperationWithAttempts( + std::bind(&RobotDriverInterface::Initialize, robot_driver_), + max_roboteq_initialization_attempts_, + std::bind(&RobotDriverInterface::Deinitialize, robot_driver_))) { + throw std::runtime_error("Roboteq drivers initialization failed."); + } + + RCLCPP_INFO(logger_, "Successfully configured robot driver"); +} + +void UGVSystem::ConfigureEStop() +{ + if (!gpio_controller_ || !roboteq_error_filter_ || !robot_driver_ || !robot_driver_write_mtx_) { + throw std::runtime_error("Failed to configure E-Stop, make sure to setup entities first."); + } + + e_stop_ = std::make_shared( + gpio_controller_, roboteq_error_filter_, robot_driver_, robot_driver_write_mtx_, + std::bind(&UGVSystem::AreVelocityCommandsNearZero, this)); + + RCLCPP_INFO(logger_, "Successfully configured E-Stop"); +} + +void UGVSystem::UpdateMotorsState() +{ + try { + robot_driver_->UpdateMotorsState(); + UpdateHwStates(); + UpdateMotorsStateDataTimedOut(); + } catch (const std::runtime_error & e) { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); + + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, steady_clock_, 5000, + "An exception occurred while updating motors states: " << e.what()); + } +} + +void UGVSystem::UpdateDriverState() +{ + try { + robot_driver_->UpdateDriversState(); + UpdateFlagErrors(); + UpdateDriverStateDataTimedOut(); + } catch (const std::runtime_error & e) { + roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); + + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, steady_clock_, 5000, + "An exception occurred while updating drivers states: " << e.what()); + } +} + +void UGVSystem::UpdateEStopState() +{ + if (robot_driver_->CommunicationError()) { + e_stop_->TriggerEStop(); + } + + const bool e_stop = e_stop_->ReadEStopState(); + system_ros_interface_->PublishEStopStateIfChanged(e_stop); +} + +void UGVSystem::HandleRobotDriverWriteOperation(std::function write_operation) +{ + try { + { + std::unique_lock robot_driver_write_lck( + *robot_driver_write_mtx_, std::defer_lock); + if (!robot_driver_write_lck.try_lock()) { + throw std::runtime_error( + "Can't acquire mutex for writing commands - E-stop is being triggered."); + } + write_operation(); + } + + roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, false); + } catch (const std::runtime_error & e) { + RCLCPP_WARN_STREAM(logger_, "An exception occurred while writing commands: " << e.what()); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true); + } +} + +bool UGVSystem::AreVelocityCommandsNearZero() +{ + for (const auto & cmd : hw_commands_velocities_) { + if (std::abs(cmd) > std::numeric_limits::epsilon()) { + return false; + } + } + return true; +} + +void UGVSystem::MotorsPowerEnable(const bool enable) +{ + try { + { + std::lock_guard lck_g(*robot_driver_write_mtx_); + + if (!enable) { + robot_driver_->TurnOnEStop(); + } else { + robot_driver_->TurnOffEStop(); + } + } + + e_stop_->TriggerEStop(); + + roboteq_error_filter_->SetClearErrorsFlag(); + roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); + } catch (const std::runtime_error & e) { + RCLCPP_WARN_STREAM(logger_, "An exception occurred while enabling motors power: " << e.what()); + } +} + +} // namespace husarion_ugv_hardware_interfaces diff --git a/panther_hardware_interfaces/src/utils.cpp b/husarion_ugv_hardware_interfaces/src/utils.cpp similarity index 93% rename from panther_hardware_interfaces/src/utils.cpp rename to husarion_ugv_hardware_interfaces/src/utils.cpp index 1620a06b..a83d3967 100644 --- a/panther_hardware_interfaces/src/utils.cpp +++ b/husarion_ugv_hardware_interfaces/src/utils.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_hardware_interfaces/utils.hpp" +#include "husarion_ugv_hardware_interfaces/utils.hpp" #include #include -namespace panther_hardware_interfaces +namespace husarion_ugv_hardware_interfaces { bool OperationWithAttempts( @@ -65,4 +65,4 @@ bool CheckIfJointNameContainValidSequence(const std::string & name, const std::s return true; } -} // namespace panther_hardware_interfaces +} // namespace husarion_ugv_hardware_interfaces diff --git a/panther_hardware_interfaces/test/config/canopen_configuration.yaml b/husarion_ugv_hardware_interfaces/test/config/canopen_configuration.yaml similarity index 60% rename from panther_hardware_interfaces/test/config/canopen_configuration.yaml rename to husarion_ugv_hardware_interfaces/test/config/canopen_configuration.yaml index 8f0a70a2..bef2f4cd 100644 --- a/panther_hardware_interfaces/test/config/canopen_configuration.yaml +++ b/husarion_ugv_hardware_interfaces/test/config/canopen_configuration.yaml @@ -6,8 +6,3 @@ slave_1: dcf: roboteq_motor_controllers_v80_21a.eds node_id: 1 heartbeat_producer: 100 - -slave_2: - dcf: roboteq_motor_controllers_v80_21a.eds - node_id: 2 - heartbeat_producer: 100 diff --git a/panther_hardware_interfaces/test/config/slave_1.bin b/husarion_ugv_hardware_interfaces/test/config/slave_1.bin similarity index 100% rename from panther_hardware_interfaces/test/config/slave_1.bin rename to husarion_ugv_hardware_interfaces/test/config/slave_1.bin diff --git a/panther_hardware_interfaces/test/panther_imu_sensor/test_panther_imu_sensor.cpp b/husarion_ugv_hardware_interfaces/test/phidget_imu_sensor/test_phidget_imu_sensor.cpp similarity index 73% rename from panther_hardware_interfaces/test/panther_imu_sensor/test_panther_imu_sensor.cpp rename to husarion_ugv_hardware_interfaces/test/phidget_imu_sensor/test_phidget_imu_sensor.cpp index 9b99f98a..d7dad3fe 100644 --- a/panther_hardware_interfaces/test/panther_imu_sensor/test_panther_imu_sensor.cpp +++ b/husarion_ugv_hardware_interfaces/test/phidget_imu_sensor/test_phidget_imu_sensor.cpp @@ -26,67 +26,67 @@ #include "lifecycle_msgs/msg/state.hpp" -#include "panther_hardware_interfaces/panther_imu_sensor/panther_imu_sensor.hpp" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_hardware_interfaces/phidget_imu_sensor/phidget_imu_sensor.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" -class PantherImuSensorWrapper : public panther_hardware_interfaces::PantherImuSensor +class PhidgetImuSensorWrapper : public husarion_ugv_hardware_interfaces::PhidgetImuSensor { public: - PantherImuSensorWrapper() {} + PhidgetImuSensorWrapper() {} void SetHardwareInfo(const hardware_interface::HardwareInfo & info) { hardware_interface::SensorInterface::info_ = info; } - void CheckSensorName() const { PantherImuSensor::CheckSensorName(); } + void CheckSensorName() const { PhidgetImuSensor::CheckSensorName(); } - void CheckStatesSize() const { PantherImuSensor::CheckStatesSize(); } + void CheckStatesSize() const { PhidgetImuSensor::CheckStatesSize(); } - void CheckInterfaces() const { PantherImuSensor::CheckInterfaces(); } + void CheckInterfaces() const { PhidgetImuSensor::CheckInterfaces(); } - void ReadObligatoryParams() { PantherImuSensor::ReadObligatoryParams(); } + void ReadObligatoryParams() { PhidgetImuSensor::ReadObligatoryParams(); } - void ReadMadgwickFilterParams() { PantherImuSensor::ReadMadgwickFilterParams(); } + void ReadMadgwickFilterParams() { PhidgetImuSensor::ReadMadgwickFilterParams(); } - void ConfigureMadgwickFilter() { PantherImuSensor::ConfigureMadgwickFilter(); } + void ConfigureMadgwickFilter() { PhidgetImuSensor::ConfigureMadgwickFilter(); } void InitializeMadgwickAlgorithm( const geometry_msgs::msg::Vector3 & mag_compensated, const geometry_msgs::msg::Vector3 & lin_acc, const rclcpp::Time timestamp) { - PantherImuSensor::InitializeMadgwickAlgorithm(mag_compensated, lin_acc, timestamp); + PhidgetImuSensor::InitializeMadgwickAlgorithm(mag_compensated, lin_acc, timestamp); } void SpatialDataCallback( const double acceleration[3], const double angular_rate[3], const double magnetic_field[3], const double timestamp) { - PantherImuSensor::SpatialDataCallback(acceleration, angular_rate, magnetic_field, timestamp); + PhidgetImuSensor::SpatialDataCallback(acceleration, angular_rate, magnetic_field, timestamp); } - void SpatialAttachCallback() { PantherImuSensor::SpatialAttachCallback(); } + void SpatialAttachCallback() { PhidgetImuSensor::SpatialAttachCallback(); } - void SpatialDetachCallback() { PantherImuSensor::SpatialDetachCallback(); } + void SpatialDetachCallback() { PhidgetImuSensor::SpatialDetachCallback(); } geometry_msgs::msg::Vector3 ParseMagnitude(const double magnetic_field[3]) { - return PantherImuSensor::ParseMagnitude(magnetic_field); + return PhidgetImuSensor::ParseMagnitude(magnetic_field); } geometry_msgs::msg::Vector3 ParseGyration(const double angular_rate[3]) { - return PantherImuSensor::ParseGyration(angular_rate); + return PhidgetImuSensor::ParseGyration(angular_rate); } geometry_msgs::msg::Vector3 ParseAcceleration(const double acceleration[3]) { - return PantherImuSensor::ParseAcceleration(acceleration); + return PhidgetImuSensor::ParseAcceleration(acceleration); } bool IsVectorFinite(const geometry_msgs::msg::Vector3 & vec) { - return PantherImuSensor::IsVectorFinite(vec); + return PhidgetImuSensor::IsVectorFinite(vec); } bool IsQuaternionFinite(const geometry_msgs::msg::Quaternion & quat) @@ -97,55 +97,55 @@ class PantherImuSensorWrapper : public panther_hardware_interfaces::PantherImuSe bool IsIMUCalibrated(const geometry_msgs::msg::Vector3 & vec) { - return PantherImuSensor::IsIMUCalibrated(vec); + return PhidgetImuSensor::IsIMUCalibrated(vec); } geometry_msgs::msg::Quaternion GetQuaternion() const { geometry_msgs::msg::Quaternion q; - q.x = imu_sensor_state_[PantherImuSensor::orientation_x]; - q.y = imu_sensor_state_[PantherImuSensor::orientation_y]; - q.z = imu_sensor_state_[PantherImuSensor::orientation_z]; - q.w = imu_sensor_state_[PantherImuSensor::orientation_w]; + q.x = imu_sensor_state_[PhidgetImuSensor::orientation_x]; + q.y = imu_sensor_state_[PhidgetImuSensor::orientation_y]; + q.z = imu_sensor_state_[PhidgetImuSensor::orientation_z]; + q.w = imu_sensor_state_[PhidgetImuSensor::orientation_w]; return q; } geometry_msgs::msg::Vector3 GetAcceleration() const { geometry_msgs::msg::Vector3 accel; - accel.x = imu_sensor_state_[PantherImuSensor::linear_acceleration_x]; - accel.y = imu_sensor_state_[PantherImuSensor::linear_acceleration_y]; - accel.z = imu_sensor_state_[PantherImuSensor::linear_acceleration_z]; + accel.x = imu_sensor_state_[PhidgetImuSensor::linear_acceleration_x]; + accel.y = imu_sensor_state_[PhidgetImuSensor::linear_acceleration_y]; + accel.z = imu_sensor_state_[PhidgetImuSensor::linear_acceleration_z]; return accel; } geometry_msgs::msg::Vector3 GetGyration() const { geometry_msgs::msg::Vector3 gyro; - gyro.x = imu_sensor_state_[PantherImuSensor::angular_velocity_x]; - gyro.y = imu_sensor_state_[PantherImuSensor::angular_velocity_y]; - gyro.z = imu_sensor_state_[PantherImuSensor::angular_velocity_z]; + gyro.x = imu_sensor_state_[PhidgetImuSensor::angular_velocity_x]; + gyro.y = imu_sensor_state_[PhidgetImuSensor::angular_velocity_y]; + gyro.z = imu_sensor_state_[PhidgetImuSensor::angular_velocity_z]; return gyro; } }; -class TestPantherImuSensor : public testing::Test +class TestPhidgetImuSensor : public testing::Test { public: - TestPantherImuSensor(); - ~TestPantherImuSensor(); + TestPhidgetImuSensor(); + ~TestPhidgetImuSensor(); void CreateResourceManagerFromUrdf(const std::string & urdf); - hardware_interface::return_type ConfigurePantherImu(); + hardware_interface::return_type ConfigurePhidgetImu(); - hardware_interface::return_type UnconfigurePantherImu(); + hardware_interface::return_type UnconfigurePhidgetImu(); - hardware_interface::return_type ActivatePantherImu(); + hardware_interface::return_type ActivatePhidgetImu(); - hardware_interface::return_type DeactivatePantherImu(); + hardware_interface::return_type DeactivatePhidgetImu(); - hardware_interface::return_type ShutdownPantherImu(); + hardware_interface::return_type ShutdownPhidgetImu(); /** * @brief Creates and returns URDF as a string @@ -156,7 +156,7 @@ class TestPantherImuSensor : public testing::Test const std::unordered_map & param_map, const std::list & interfaces_list); - std::string GetDefaultPantherImuUrdf(); + std::string GetDefaultPhidgetImuUrdf(); protected: /** @@ -179,7 +179,7 @@ class TestPantherImuSensor : public testing::Test std::list ClaimGoodStateInterfaces(); - inline static const std::string kPantherImuName = "imu"; + inline static const std::string kPhidgetImuName = "imu"; inline static const std::string kUrdfHeader = R"( @@ -206,61 +206,61 @@ class TestPantherImuSensor : public testing::Test {"world_frame", "enu"}}; inline static const std::string kPluginName = - "panther_hardware_interfaces/PantherImuSensor"; + "husarion_ugv_hardware_interfaces/PhidgetImuSensor"; - std::unique_ptr imu_sensor_; + std::unique_ptr imu_sensor_; std::shared_ptr rm_; }; -TestPantherImuSensor::TestPantherImuSensor() +TestPhidgetImuSensor::TestPhidgetImuSensor() { - imu_sensor_ = std::make_unique(); + imu_sensor_ = std::make_unique(); rclcpp::init(0, nullptr); } -TestPantherImuSensor::~TestPantherImuSensor() { rclcpp::shutdown(); } +TestPhidgetImuSensor::~TestPhidgetImuSensor() { rclcpp::shutdown(); } -void TestPantherImuSensor::CreateResourceManagerFromUrdf(const std::string & urdf) +void TestPhidgetImuSensor::CreateResourceManagerFromUrdf(const std::string & urdf) { rm_ = std::make_shared(urdf); } -hardware_interface::return_type TestPantherImuSensor::ConfigurePantherImu() +hardware_interface::return_type TestPhidgetImuSensor::ConfigurePhidgetImu() { return SetState( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); } -hardware_interface::return_type TestPantherImuSensor::UnconfigurePantherImu() +hardware_interface::return_type TestPhidgetImuSensor::UnconfigurePhidgetImu() { return SetState( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED); } -hardware_interface::return_type TestPantherImuSensor::ActivatePantherImu() +hardware_interface::return_type TestPhidgetImuSensor::ActivatePhidgetImu() { return SetState( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); } -hardware_interface::return_type TestPantherImuSensor::DeactivatePantherImu() +hardware_interface::return_type TestPhidgetImuSensor::DeactivatePhidgetImu() { return SetState( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); } -hardware_interface::return_type TestPantherImuSensor::ShutdownPantherImu() +hardware_interface::return_type TestPhidgetImuSensor::ShutdownPhidgetImu() { return SetState( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, hardware_interface::lifecycle_state_names::FINALIZED); } -std::string TestPantherImuSensor::BuildUrdf( +std::string TestPhidgetImuSensor::BuildUrdf( const std::unordered_map & param_map, const std::list & interfaces_list) { @@ -286,18 +286,18 @@ std::string TestPantherImuSensor::BuildUrdf( return urdf.str(); } -std::string TestPantherImuSensor::GetDefaultPantherImuUrdf() +std::string TestPhidgetImuSensor::GetDefaultPhidgetImuUrdf() { return BuildUrdf(kImuObligatoryParams, kImuInterfaces); } -hardware_interface::return_type TestPantherImuSensor::SetState( +hardware_interface::return_type TestPhidgetImuSensor::SetState( const std::uint8_t state_id, const std::string & state_name) { rclcpp_lifecycle::State state(state_id, state_name); - return rm_->set_component_state(kPantherImuName, state); + return rm_->set_component_state(kPhidgetImuName, state); } -hardware_interface::HardwareInfo TestPantherImuSensor::CreateExampleInterfaces( +hardware_interface::HardwareInfo TestPhidgetImuSensor::CreateExampleInterfaces( const hardware_interface::HardwareInfo & info) { hardware_interface::HardwareInfo new_info(info); @@ -312,11 +312,11 @@ hardware_interface::HardwareInfo TestPantherImuSensor::CreateExampleInterfaces( return new_info; } -hardware_interface::HardwareInfo TestPantherImuSensor::CreateCorrectInterfaces( +hardware_interface::HardwareInfo TestPhidgetImuSensor::CreateCorrectInterfaces( const hardware_interface::HardwareInfo & info) { hardware_interface::HardwareInfo new_info(info); - for (const auto & interface_name : TestPantherImuSensor::kImuInterfaces) { + for (const auto & interface_name : TestPhidgetImuSensor::kImuInterfaces) { hardware_interface::InterfaceInfo interface_info; interface_info.name = interface_name; new_info.sensors.front().state_interfaces.push_back(interface_info); @@ -324,7 +324,7 @@ hardware_interface::HardwareInfo TestPantherImuSensor::CreateCorrectInterfaces( return new_info; } -hardware_interface::HardwareInfo TestPantherImuSensor::AddMadgwickParameters( +hardware_interface::HardwareInfo TestPhidgetImuSensor::AddMadgwickParameters( const hardware_interface::HardwareInfo & info) { hardware_interface::HardwareInfo new_info(info); @@ -340,16 +340,16 @@ hardware_interface::HardwareInfo TestPantherImuSensor::AddMadgwickParameters( return new_info; } -std::list TestPantherImuSensor::ClaimGoodStateInterfaces() +std::list TestPhidgetImuSensor::ClaimGoodStateInterfaces() { std::list list; for (const auto & interface_name : kImuInterfaces) { - list.push_back(rm_->claim_state_interface(kPantherImuName + "/" + interface_name)); + list.push_back(rm_->claim_state_interface(kPhidgetImuName + "/" + interface_name)); } return list; } -TEST_F(TestPantherImuSensor, CheckSensorName) +TEST_F(TestPhidgetImuSensor, CheckSensorName) { hardware_interface::HardwareInfo info; imu_sensor_->SetHardwareInfo(info); @@ -363,7 +363,7 @@ TEST_F(TestPantherImuSensor, CheckSensorName) EXPECT_NO_THROW({ imu_sensor_->CheckSensorName(); }); } -TEST_F(TestPantherImuSensor, CheckStatesSize) +TEST_F(TestPhidgetImuSensor, CheckStatesSize) { hardware_interface::HardwareInfo info; info.sensors.push_back({}); @@ -377,7 +377,7 @@ TEST_F(TestPantherImuSensor, CheckStatesSize) EXPECT_NO_THROW({ imu_sensor_->CheckStatesSize(); }); } -TEST_F(TestPantherImuSensor, CheckInterfaces) +TEST_F(TestPhidgetImuSensor, CheckInterfaces) { hardware_interface::HardwareInfo info; info.sensors.push_back({}); @@ -395,25 +395,25 @@ TEST_F(TestPantherImuSensor, CheckInterfaces) EXPECT_NO_THROW({ imu_sensor_->CheckInterfaces(); }); } -TEST_F(TestPantherImuSensor, ReadObligatoryParams) +TEST_F(TestPhidgetImuSensor, ReadObligatoryParams) { hardware_interface::HardwareInfo info; info.hardware_parameters = {{"param1", "value1"}, {"param2", "value2"}}; imu_sensor_->SetHardwareInfo(info); EXPECT_THROW({ imu_sensor_->ReadObligatoryParams(); }, std::exception); - info.hardware_parameters = TestPantherImuSensor::kImuObligatoryParams; + info.hardware_parameters = TestPhidgetImuSensor::kImuObligatoryParams; info.hardware_parameters["callback_delta_epsilon_ms"] = info.hardware_parameters["data_interval_ms"]; imu_sensor_->SetHardwareInfo(info); EXPECT_THROW({ imu_sensor_->ReadObligatoryParams(); }, std::exception); - info.hardware_parameters = TestPantherImuSensor::kImuObligatoryParams; + info.hardware_parameters = TestPhidgetImuSensor::kImuObligatoryParams; imu_sensor_->SetHardwareInfo(info); EXPECT_NO_THROW({ imu_sensor_->ReadObligatoryParams(); }); } -TEST_F(TestPantherImuSensor, CheckMagnitudeWrongValueAndCalibration) +TEST_F(TestPhidgetImuSensor, CheckMagnitudeWrongValueAndCalibration) { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; hardware_interface::HardwareInfo info; @@ -425,7 +425,7 @@ TEST_F(TestPantherImuSensor, CheckMagnitudeWrongValueAndCalibration) ASSERT_EQ(imu_sensor_->on_init(info), CallbackReturn::SUCCESS); double magnitude[3]; - magnitude[0] = panther_hardware_interfaces::PantherImuSensor::KImuMagneticFieldUnknownValue; + magnitude[0] = husarion_ugv_hardware_interfaces::PhidgetImuSensor::KImuMagneticFieldUnknownValue; const auto wrong_magnitude_parsed = imu_sensor_->ParseMagnitude(magnitude); ASSERT_FALSE(imu_sensor_->IsVectorFinite(wrong_magnitude_parsed)); @@ -439,7 +439,7 @@ TEST_F(TestPantherImuSensor, CheckMagnitudeWrongValueAndCalibration) ASSERT_TRUE(imu_sensor_->IsIMUCalibrated(magnitude_parsed)); } -TEST_F(TestPantherImuSensor, CheckCalibrationOnDataCallbackAndAlgorithmInitialization) +TEST_F(TestPhidgetImuSensor, CheckCalibrationOnDataCallbackAndAlgorithmInitialization) { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; hardware_interface::HardwareInfo info; @@ -451,7 +451,7 @@ TEST_F(TestPantherImuSensor, CheckCalibrationOnDataCallbackAndAlgorithmInitializ ASSERT_EQ(imu_sensor_->on_init(info), CallbackReturn::SUCCESS); double magnitude[3], acceleration[3], gyration[3]; - magnitude[0] = panther_hardware_interfaces::PantherImuSensor::KImuMagneticFieldUnknownValue; + magnitude[0] = husarion_ugv_hardware_interfaces::PhidgetImuSensor::KImuMagneticFieldUnknownValue; const auto fake_wrong_magnitude_parsed = imu_sensor_->ParseMagnitude(magnitude); ASSERT_FALSE(imu_sensor_->IsIMUCalibrated(fake_wrong_magnitude_parsed)); @@ -481,7 +481,7 @@ TEST_F(TestPantherImuSensor, CheckCalibrationOnDataCallbackAndAlgorithmInitializ ASSERT_TRUE(imu_sensor_->IsVectorFinite(imu_sensor_->GetGyration())); } -TEST_F(TestPantherImuSensor, CheckReconnection) +TEST_F(TestPhidgetImuSensor, CheckReconnection) { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; hardware_interface::HardwareInfo info; @@ -493,7 +493,7 @@ TEST_F(TestPantherImuSensor, CheckReconnection) ASSERT_EQ(imu_sensor_->on_init(info), CallbackReturn::SUCCESS); double magnitude[3], acceleration[3], gyration[3]; - magnitude[0] = panther_hardware_interfaces::PantherImuSensor::KImuMagneticFieldUnknownValue; + magnitude[0] = husarion_ugv_hardware_interfaces::PhidgetImuSensor::KImuMagneticFieldUnknownValue; // Correct values read from sensor magnitude[0] = -0.09675; @@ -530,9 +530,9 @@ TEST_F(TestPantherImuSensor, CheckReconnection) ASSERT_TRUE(imu_sensor_->IsVectorFinite(imu_sensor_->GetGyration())); } -TEST_F(TestPantherImuSensor, CheckInterfacesLoadedByResourceManager) +TEST_F(TestPhidgetImuSensor, CheckInterfacesLoadedByResourceManager) { - CreateResourceManagerFromUrdf(GetDefaultPantherImuUrdf()); + CreateResourceManagerFromUrdf(GetDefaultPhidgetImuUrdf()); EXPECT_EQ(rm_->sensor_components_size(), 1u); ASSERT_EQ(rm_->state_interface_keys().size(), 10u); @@ -551,14 +551,14 @@ TEST_F(TestPantherImuSensor, CheckInterfacesLoadedByResourceManager) EXPECT_TRUE(rm_->state_interface_exists("imu/linear_acceleration.z")); } -TEST_F(TestPantherImuSensor, CheckStatesInitialValues) +TEST_F(TestPhidgetImuSensor, CheckStatesInitialValues) { using hardware_interface::LoanedStateInterface; using hardware_interface::return_type; - CreateResourceManagerFromUrdf(GetDefaultPantherImuUrdf()); + CreateResourceManagerFromUrdf(GetDefaultPhidgetImuUrdf()); - ASSERT_EQ(ConfigurePantherImu(), return_type::OK); + ASSERT_EQ(ConfigurePhidgetImu(), return_type::OK); auto loaded_state_interfaces = ClaimGoodStateInterfaces(); @@ -566,29 +566,29 @@ TEST_F(TestPantherImuSensor, CheckStatesInitialValues) EXPECT_TRUE(std::isnan(state_interface.get_value())); } - EXPECT_EQ(ShutdownPantherImu(), return_type::OK); + EXPECT_EQ(ShutdownPhidgetImu(), return_type::OK); } -TEST_F(TestPantherImuSensor, CheckWrongConfigurationWithWrongParameters) +TEST_F(TestPhidgetImuSensor, CheckWrongConfigurationWithWrongParameters) { using hardware_interface::return_type; - const std::string panther_system_urdf_ = BuildUrdf({}, TestPantherImuSensor::kImuInterfaces); - CreateResourceManagerFromUrdf(panther_system_urdf_); + const std::string robot_system_urdf_ = BuildUrdf({}, TestPhidgetImuSensor::kImuInterfaces); + CreateResourceManagerFromUrdf(robot_system_urdf_); - EXPECT_EQ(ConfigurePantherImu(), return_type::ERROR); - EXPECT_EQ(ShutdownPantherImu(), return_type::OK); + EXPECT_EQ(ConfigurePhidgetImu(), return_type::ERROR); + EXPECT_EQ(ShutdownPhidgetImu(), return_type::OK); } -TEST_F(TestPantherImuSensor, CheckReadAndConfigureRealSensor) +TEST_F(TestPhidgetImuSensor, CheckReadAndConfigureRealSensor) { using hardware_interface::LoanedStateInterface; using hardware_interface::return_type; - CreateResourceManagerFromUrdf(GetDefaultPantherImuUrdf()); - EXPECT_EQ(ConfigurePantherImu(), return_type::OK); + CreateResourceManagerFromUrdf(GetDefaultPhidgetImuUrdf()); + EXPECT_EQ(ConfigurePhidgetImu(), return_type::OK); - ASSERT_EQ(ActivatePantherImu(), return_type::OK); + ASSERT_EQ(ActivatePhidgetImu(), return_type::OK); std::this_thread::sleep_for(std::chrono::milliseconds(100)); auto loaded_state_interfaces = ClaimGoodStateInterfaces(); @@ -596,8 +596,8 @@ TEST_F(TestPantherImuSensor, CheckReadAndConfigureRealSensor) EXPECT_TRUE(std::isfinite(state_interface.get_value())); } - EXPECT_EQ(UnconfigurePantherImu(), return_type::OK); - EXPECT_EQ(ShutdownPantherImu(), return_type::OK); + EXPECT_EQ(UnconfigurePhidgetImu(), return_type::OK); + EXPECT_EQ(ShutdownPhidgetImu(), return_type::OK); } int main(int argc, char ** argv) diff --git a/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp new file mode 100644 index 00000000..89ce4cc7 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp @@ -0,0 +1,201 @@ +// 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 + +using GPIOInfo = husarion_ugv_hardware_interfaces::GPIOInfo; +using GPIOPin = husarion_ugv_hardware_interfaces::GPIOPin; + +class MockGPIODriver : public husarion_ugv_hardware_interfaces::GPIODriverInterface +{ +public: + MOCK_METHOD( + void, GPIOMonitorEnable, (const bool use_rt, const unsigned gpio_monit_thread_sched_priority), + (override)); + MOCK_METHOD( + void, ConfigureEdgeEventCallback, (const std::function & callback), + (override)); + MOCK_METHOD( + void, ChangePinDirection, (const GPIOPin pin, const gpiod::line::direction direction), + (override)); + MOCK_METHOD(bool, IsPinAvailable, (const GPIOPin pin), (const, override)); + MOCK_METHOD(bool, IsPinActive, (const GPIOPin pin), (override)); + MOCK_METHOD( + bool, SetPinValue, + (const GPIOPin pin, const bool value, + const std::chrono::milliseconds & pin_validation_wait_time), + (override)); + + using NiceMock = testing::NiceMock; +}; + +class GPIOControllerWrapper : public husarion_ugv_hardware_interfaces::GPIOController +{ +public: + GPIOControllerWrapper(std::shared_ptr gpio_driver) + : husarion_ugv_hardware_interfaces::GPIOController(gpio_driver) + { + } + + void WatchdogEnable() { this->watchdog_->TurnOn(); } + void WatchdogDisable() { this->watchdog_->TurnOff(); } + bool IsWatchdogEnabled() { return this->watchdog_->IsWatchdogEnabled(); } +}; + +class TestGPIOController : public ::testing::Test +{ +public: + TestGPIOController(); + ~TestGPIOController() { gpio_controller_wrapper_.reset(); } + +protected: + float GetRobotVersion(); + + std::shared_ptr gpio_driver_; + std::unique_ptr gpio_controller_wrapper_; + static constexpr int watchdog_edges_per_100ms_ = 10; +}; + +TestGPIOController::TestGPIOController() +{ + gpio_driver_ = std::make_shared(); + + // Mock methods called during the initialization process + ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::VMOT_ON, true, testing::_)) + .WillByDefault(testing::Return(true)); + ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::DRIVER_EN, true, testing::_)) + .WillByDefault(testing::Return(true)); + ON_CALL(*gpio_driver_, IsPinAvailable(GPIOPin::WATCHDOG)).WillByDefault(testing::Return(true)); + + gpio_controller_wrapper_ = std::make_unique(gpio_driver_); + gpio_controller_wrapper_->Start(); +} + +struct GPIOTestParam +{ + GPIOPin pin; + std::function enable_method; + bool is_inverted = false; +}; + +class ParametrizedTestGPIOController : public TestGPIOController, + public ::testing::WithParamInterface +{ +}; + +TEST(TestGPIOControllerInitialization, GPIODriverUninitialized) +{ + std::shared_ptr gpio_driver; + + EXPECT_THROW(std::make_unique(gpio_driver), std::runtime_error); +} + +TEST(TestGPIOControllerInitialization, WatchdogPinNotAvailable) +{ + auto gpio_driver = std::make_shared(); + + EXPECT_CALL(*gpio_driver, SetPinValue(testing::_, true, testing::_)) + .Times(2) + .WillRepeatedly(testing::Return(true)); + ON_CALL(*gpio_driver, IsPinAvailable(GPIOPin::WATCHDOG)).WillByDefault(testing::Return(false)); + + auto gpio_controller_wrapper = std::make_unique(gpio_driver); + + EXPECT_THROW(gpio_controller_wrapper->Start(), std::runtime_error); +} + +TEST_F(TestGPIOController, TestMotorsInitSuccess) +{ + ON_CALL(*gpio_driver_, IsPinActive(GPIOPin::VMOT_ON)).WillByDefault(testing::Return(true)); + ON_CALL(*gpio_driver_, IsPinActive(GPIOPin::DRIVER_EN)).WillByDefault(testing::Return(true)); + + EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::VMOT_ON)); + EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::DRIVER_EN)); +} + +TEST_F(TestGPIOController, TestEStopResetAlreadyDeactivated) +{ + // The E_STOP_RESET pin is already deactivated + EXPECT_CALL(*gpio_driver_, IsPinActive(GPIOPin::E_STOP_RESET)).WillOnce(testing::Return(true)); + + gpio_controller_wrapper_->EStopReset(); +} + +TEST_P(ParametrizedTestGPIOController, TestGPIOEnableDisable) +{ + const auto & param = GetParam(); + + // Set the enable command based on the pin inversion + auto const enable = param.is_inverted ? false : true; + auto const disable = !enable; + + // Enable GPIO + EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, enable, testing::_)) + .WillOnce(testing::Return(true)); + EXPECT_CALL(*gpio_driver_, IsPinActive(param.pin)).WillOnce(testing::Return(enable)); + + param.enable_method(gpio_controller_wrapper_.get(), true); + EXPECT_EQ(enable, gpio_controller_wrapper_->IsPinActive(param.pin)); + + // Disable GPIO + EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, disable, testing::_)) + .WillOnce(testing::Return(true)); + EXPECT_CALL(*gpio_driver_, IsPinActive(param.pin)).WillOnce(testing::Return(disable)); + + param.enable_method(gpio_controller_wrapper_.get(), false); + EXPECT_EQ(disable, gpio_controller_wrapper_->IsPinActive(param.pin)); +} + +INSTANTIATE_TEST_SUITE_P( + GPIOTests, ParametrizedTestGPIOController, + ::testing::Values( + GPIOTestParam{GPIOPin::DRIVER_EN, &GPIOControllerWrapper::MotorPowerEnable}, + GPIOTestParam{GPIOPin::AUX_PW_EN, &GPIOControllerWrapper::AUXPowerEnable}, + GPIOTestParam{GPIOPin::FAN_SW, &GPIOControllerWrapper::FanEnable}, + GPIOTestParam{GPIOPin::VDIG_OFF, &GPIOControllerWrapper::DigitalPowerEnable, true}, + GPIOTestParam{GPIOPin::CHRG_DISABLE, &GPIOControllerWrapper::ChargerEnable, true}, + GPIOTestParam{GPIOPin::LED_SBC_SEL, &GPIOControllerWrapper::LEDControlEnable})); + +TEST_F(TestGPIOController, TestQueryControlInterfaceIOStates) +{ + EXPECT_CALL(*gpio_driver_, IsPinActive(testing::_)) + .Times(7) + .WillRepeatedly(testing::Return(true)); + + std::unordered_map io_states = + gpio_controller_wrapper_->QueryControlInterfaceIOStates(); + + ASSERT_EQ(io_states.size(), 7); + + for (const auto & [pin, state] : io_states) { + EXPECT_TRUE(state); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + auto result = RUN_ALL_TESTS(); + + return result; +} diff --git a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_driver.cpp similarity index 68% rename from panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp rename to husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_driver.cpp index f8f787f2..2bff4cab 100644 --- a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_driver.cpp +++ b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_driver.cpp @@ -21,14 +21,14 @@ #include #include -#include "gpiod.hpp" -#include "gtest/gtest.h" +#include +#include -#include "panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" -using GPIOInfo = panther_hardware_interfaces::GPIOInfo; -using GPIOPin = panther_hardware_interfaces::GPIOPin; +using GPIOInfo = husarion_ugv_hardware_interfaces::GPIOInfo; +using GPIOPin = husarion_ugv_hardware_interfaces::GPIOPin; class TestGPIODriver : public ::testing::Test { @@ -43,7 +43,7 @@ class TestGPIODriver : public ::testing::Test void TearDown() override; void SetAndVerifyPinState(const GPIOPin & pin); - std::unique_ptr gpio_driver_; + std::unique_ptr gpio_driver_; std::pair last_gpio_event_summary_; const std::vector gpio_config_info_{ GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT}, @@ -52,7 +52,7 @@ class TestGPIODriver : public ::testing::Test void TestGPIODriver::SetUp() { - gpio_driver_ = std::make_unique(gpio_config_info_); + gpio_driver_ = std::make_unique(gpio_config_info_); last_gpio_event_summary_.first = static_cast(-1); last_gpio_event_summary_.second = static_cast(-1); @@ -80,7 +80,7 @@ TEST(TestGPIODriverInitialization, EmptyInfoStorage) EXPECT_THROW( { auto gpio_driver = - std::make_unique(std::vector{}); + std::make_unique(std::vector{}); }, std::runtime_error); } @@ -90,11 +90,11 @@ TEST(TestGPIODriverInitialization, WrongPinConfigFail) // There is no OS version that supports simultaneous operation of MOTOR_ON and VMOT_ON pins. EXPECT_THROW( { - auto gpio_driver = std::make_unique( + auto gpio_driver = std::make_unique( std::vector{{GPIOPin::MOTOR_ON, gpiod::line::direction::OUTPUT}}); gpio_driver.reset(); - gpio_driver = std::make_unique( + gpio_driver = std::make_unique( std::vector{{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}}); }, std::invalid_argument); @@ -102,15 +102,41 @@ TEST(TestGPIODriverInitialization, WrongPinConfigFail) TEST_F(TestGPIODriver, SetWrongPinValue) { - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + auto is_message_thrown = husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { this->gpio_driver_->SetPinValue(static_cast(-1), true); }, - "Pin not found in GPIO info storage.")); + "Pin not found in GPIO info storage."); + + EXPECT_TRUE(is_message_thrown); +} + +TEST_F(TestGPIODriver, IsPinAvailableValidPin) +{ + auto is_pin_available = this->gpio_driver_->IsPinAvailable(GPIOPin::LED_SBC_SEL); + + EXPECT_TRUE(is_pin_available); +} + +TEST_F(TestGPIODriver, IsPinAvailableInvalidPin) +{ + auto is_pin_available = this->gpio_driver_->IsPinAvailable(static_cast(-1)); + + EXPECT_FALSE(is_pin_available); +} + +TEST_F(TestGPIODriver, IsPinActive) +{ + this->gpio_driver_->GPIOMonitorEnable(); + + SetAndVerifyPinState(GPIOPin::LED_SBC_SEL); } -TEST_F(TestGPIODriver, IsPinAvailable) +TEST_F(TestGPIODriver, IsPinActiveNoMonitorThread) { - EXPECT_TRUE(this->gpio_driver_->IsPinAvailable(GPIOPin::LED_SBC_SEL)); - EXPECT_FALSE(this->gpio_driver_->IsPinAvailable(static_cast(-1))); + auto is_message_thrown = husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { this->gpio_driver_->IsPinActive(GPIOPin::LED_SBC_SEL); }, + "GPIO monitor thread is not running!"); + + EXPECT_TRUE(is_message_thrown); } TEST_F(TestGPIODriver, GPIOMonitorEnableNoRT) @@ -138,14 +164,16 @@ TEST_F(TestGPIODriver, GPIOMonitorEnableUseRT) SetAndVerifyPinState(GPIOPin::LED_SBC_SEL); } -TEST_F(TestGPIODriver, GPIOEventCallbackFailWhenNoMonitorThread) +TEST_F(TestGPIODriver, GPIOEventCallbackNoMonitorThread) { - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + auto is_message_thrown = husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { this->gpio_driver_->ConfigureEdgeEventCallback( std::bind(&TestGPIODriver::GPIOEventCallback, this, std::placeholders::_1)); }, - "GPIO monitor thread is not running!")); + "GPIO monitor thread is not running!"); + + EXPECT_TRUE(is_message_thrown); } TEST_F(TestGPIODriver, GPIOEventCallbackShareNewPinState) @@ -174,9 +202,11 @@ TEST_F(TestGPIODriver, ChangePinDirection) this->gpio_driver_->GPIOMonitorEnable(); this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::INPUT); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + auto is_message_thrown = husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { this->gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, true); }, - "Cannot set value for INPUT pin.")); + "Cannot set value for INPUT pin."); + + ASSERT_TRUE(is_message_thrown); this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT); @@ -186,5 +216,8 @@ TEST_F(TestGPIODriver, ChangePinDirection) int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_hardware_interfaces/test/test_utils.cpp b/husarion_ugv_hardware_interfaces/test/test_utils.cpp similarity index 86% rename from panther_hardware_interfaces/test/test_utils.cpp rename to husarion_ugv_hardware_interfaces/test/test_utils.cpp index 43f8c457..8d6f0f88 100644 --- a/panther_hardware_interfaces/test/test_utils.cpp +++ b/husarion_ugv_hardware_interfaces/test/test_utils.cpp @@ -19,11 +19,11 @@ #include -#include +#include TEST(TestUtils, GetByte) { - using panther_hardware_interfaces::GetByte; + using husarion_ugv_hardware_interfaces::GetByte; EXPECT_EQ(GetByte(static_cast(0xFA3B4186), 0), 0x86); EXPECT_EQ(GetByte(static_cast(0xFA3B4186), 1), 0x41); @@ -33,7 +33,7 @@ TEST(TestUtils, GetByte) TEST(TestUtils, GetByteOutOfRange) { - using panther_hardware_interfaces::GetByte; + using husarion_ugv_hardware_interfaces::GetByte; EXPECT_THROW(GetByte(static_cast(0xFA3B4186), 4), std::runtime_error); EXPECT_THROW(GetByte(static_cast(0xFA3B4186), -1), std::runtime_error); @@ -45,7 +45,7 @@ TEST(TestUtils, OperationWithAttemptsFailTest) unsigned attempts_counter = 0; unsigned on_error_counter = 0; - EXPECT_FALSE(panther_hardware_interfaces::OperationWithAttempts( + EXPECT_FALSE(husarion_ugv_hardware_interfaces::OperationWithAttempts( [&attempts_counter]() { ++attempts_counter; throw std::runtime_error(""); @@ -60,7 +60,7 @@ TEST(TestUtils, OperationWithAttemptsSuccessTest) unsigned max_attempts = 5; unsigned attempts_counter = 0; - EXPECT_TRUE(panther_hardware_interfaces::OperationWithAttempts( + EXPECT_TRUE(husarion_ugv_hardware_interfaces::OperationWithAttempts( [&attempts_counter, &max_attempts]() { ++attempts_counter; if (attempts_counter < max_attempts) { @@ -73,13 +73,13 @@ TEST(TestUtils, OperationWithAttemptsSuccessTest) TEST(TestUtils, OperationWithAttemptsOnErrorThrowTest) { - EXPECT_FALSE(panther_hardware_interfaces::OperationWithAttempts( + EXPECT_FALSE(husarion_ugv_hardware_interfaces::OperationWithAttempts( []() { throw std::runtime_error(""); }, 5, []() { throw std::runtime_error(""); })); } TEST(TestUtils, CheckIfJointNameContainValidSequence) { - using panther_hardware_interfaces::CheckIfJointNameContainValidSequence; + using husarion_ugv_hardware_interfaces::CheckIfJointNameContainValidSequence; EXPECT_TRUE(CheckIfJointNameContainValidSequence("fr_wheel_joint", "fr")); EXPECT_TRUE(CheckIfJointNameContainValidSequence("namespace/fr_wheel_joint", "fr")); diff --git a/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_canopen_manager.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_canopen_manager.cpp new file mode 100644 index 00000000..6db65c90 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_canopen_manager.cpp @@ -0,0 +1,101 @@ +// Copyright 2023 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 "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" + +#include "utils/fake_can_socket.hpp" +#include "utils/test_constants.hpp" + +#include "husarion_ugv_utils/test/test_utils.hpp" + +class TestCANopenManager : public ::testing::Test +{ +public: + TestCANopenManager(); + + ~TestCANopenManager() {} + +protected: + std::unique_ptr can_socket_; + std::unique_ptr canopen_manager_; +}; + +TestCANopenManager::TestCANopenManager() +{ + can_socket_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name); + + canopen_manager_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings); +} + +TEST_F(TestCANopenManager, InitializeAndDeinitialize) +{ + ASSERT_NO_THROW(canopen_manager_->Initialize()); + ASSERT_NO_THROW(canopen_manager_->Deinitialize()); + + // Check if deinitialization worked correctly - initialize once again + ASSERT_NO_THROW(canopen_manager_->Initialize()); +} + +TEST_F(TestCANopenManager, Activate) +{ + can_socket_->Initialize(); + + ASSERT_NO_THROW(canopen_manager_->Initialize()); + EXPECT_NO_THROW(canopen_manager_->Activate()); + ASSERT_NO_THROW(canopen_manager_->Deinitialize()); + + // Check if deinitialization worked correctly - activate once again + ASSERT_NO_THROW(canopen_manager_->Initialize()); + EXPECT_NO_THROW(canopen_manager_->Activate()); +} + +TEST_F(TestCANopenManager, ActivateNotInitialized) +{ + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { canopen_manager_->Activate(); }, "CANopenManager not initialized.")); +} + +TEST_F(TestCANopenManager, ActivateNoCommunication) +{ + ASSERT_NO_THROW(canopen_manager_->Initialize()); + EXPECT_THROW(canopen_manager_->Activate(), std::runtime_error); +} + +TEST_F(TestCANopenManager, GetMaster) +{ + can_socket_->Initialize(); + ASSERT_NO_THROW(canopen_manager_->Initialize()); + EXPECT_NO_THROW(canopen_manager_->GetMaster()); +} + +TEST_F(TestCANopenManager, GetMasterNotInitialized) +{ + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { canopen_manager_->GetMaster(); }, "CANopenManager not initialized.")); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_lynx_robot_driver.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_lynx_robot_driver.cpp new file mode 100644 index 00000000..59918387 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_lynx_robot_driver.cpp @@ -0,0 +1,149 @@ +// Copyright 2023 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 +#include +#include +#include + +#include "utils/fake_can_socket.hpp" +#include "utils/mock_driver.hpp" +#include "utils/test_constants.hpp" + +#include "husarion_ugv_utils/test/test_utils.hpp" + +class LynxRobotDriverWrapper : public husarion_ugv_hardware_interfaces::LynxRobotDriver +{ +public: + LynxRobotDriverWrapper( + const husarion_ugv_hardware_interfaces::CANopenSettings & canopen_settings, + const husarion_ugv_hardware_interfaces::DrivetrainSettings & drivetrain_settings, + const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000)) + : LynxRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time) + { + mock_left_motor_driver = std::make_shared< + ::testing::NiceMock>(); + mock_right_motor_driver = std::make_shared< + ::testing::NiceMock>(); + + mock_driver = + std::make_shared<::testing::NiceMock>(); + mock_driver->AddMotorDriver( + husarion_ugv_hardware_interfaces::MotorNames::LEFT, mock_left_motor_driver); + mock_driver->AddMotorDriver( + husarion_ugv_hardware_interfaces::MotorNames::RIGHT, mock_right_motor_driver); + } + + void DefineDrivers() override + { + drivers_.emplace(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT, mock_driver); + } + + std::shared_ptr<::testing::NiceMock> + mock_driver; + std::shared_ptr<::testing::NiceMock> + mock_left_motor_driver; + std::shared_ptr<::testing::NiceMock> + mock_right_motor_driver; +}; + +class TestLynxRobotDriver : public ::testing::Test +{ +public: + TestLynxRobotDriver() + { + can_socket_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name); + can_socket_->Initialize(); + + robot_driver_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings, + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10)); + + robot_driver_->Initialize(); + robot_driver_->Activate(); + } + + ~TestLynxRobotDriver() { robot_driver_->Deinitialize(); } + +protected: + std::unique_ptr can_socket_; + std::unique_ptr robot_driver_; +}; + +TEST_F(TestLynxRobotDriver, SendSpeedCommands) +{ + using husarion_ugv_hardware_interfaces_test::kRadPerSecToRbtqCmd; + + const float left_v = 0.1; + const float right_v = 0.2; + + EXPECT_CALL(*robot_driver_->mock_driver, IsCANError()).Times(1); + EXPECT_CALL( + *robot_driver_->mock_left_motor_driver, + SendCmdVel(::testing::Eq(static_cast(left_v * kRadPerSecToRbtqCmd)))) + .Times(1); + EXPECT_CALL( + *robot_driver_->mock_right_motor_driver, + SendCmdVel(::testing::Eq(static_cast(right_v * kRadPerSecToRbtqCmd)))) + .Times(1); + + const std::vector speeds = {left_v, right_v}; + EXPECT_NO_THROW(robot_driver_->SendSpeedCommands(speeds)); +} + +TEST_F(TestLynxRobotDriver, SendSpeedCommandsSendCmdVelError) +{ + EXPECT_CALL(*robot_driver_->mock_left_motor_driver, SendCmdVel(::testing::_)) + .WillOnce(::testing::Throw(std::runtime_error(""))); + + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0}); }, "Driver send Roboteq cmd failed:")); +} + +TEST_F(TestLynxRobotDriver, SendSpeedCommandsCANError) +{ + EXPECT_CALL(*robot_driver_->mock_driver, IsCANError()).WillOnce(::testing::Return(true)); + + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0}); }, + "CAN error detected on the Driver when trying to write speed commands.")); +} + +TEST_F(TestLynxRobotDriver, SendSpeedCommandsInvalidVectorSize) +{ + const std::vector speeds = {0.1, 0.2, 0.3}; + + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { robot_driver_->SendSpeedCommands(speeds); }, "Invalid speeds vector size")); +} + +TEST_F(TestLynxRobotDriver, AttemptErrorFlagReset) +{ + EXPECT_NO_THROW(robot_driver_->AttemptErrorFlagReset()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_panther_robot_driver.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_panther_robot_driver.cpp new file mode 100644 index 00000000..4663958e --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_panther_robot_driver.cpp @@ -0,0 +1,179 @@ +// Copyright 2023 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 +#include +#include +#include + +#include "utils/fake_can_socket.hpp" +#include "utils/mock_driver.hpp" +#include "utils/test_constants.hpp" + +#include "husarion_ugv_utils/test/test_utils.hpp" + +class PantherRobotDriverWrapper : public husarion_ugv_hardware_interfaces::PantherRobotDriver +{ +public: + PantherRobotDriverWrapper( + const husarion_ugv_hardware_interfaces::CANopenSettings & canopen_settings, + const husarion_ugv_hardware_interfaces::DrivetrainSettings & drivetrain_settings, + const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000)) + : PantherRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time) + { + mock_fl_motor_driver = std::make_shared< + ::testing::NiceMock>(); + mock_fr_motor_driver = std::make_shared< + ::testing::NiceMock>(); + mock_rl_motor_driver = std::make_shared< + ::testing::NiceMock>(); + mock_rr_motor_driver = std::make_shared< + ::testing::NiceMock>(); + + mock_front_driver = + std::make_shared<::testing::NiceMock>(); + mock_front_driver->AddMotorDriver( + husarion_ugv_hardware_interfaces::MotorNames::LEFT, mock_fl_motor_driver); + mock_front_driver->AddMotorDriver( + husarion_ugv_hardware_interfaces::MotorNames::RIGHT, mock_fr_motor_driver); + + mock_rear_driver = + std::make_shared<::testing::NiceMock>(); + mock_rear_driver->AddMotorDriver( + husarion_ugv_hardware_interfaces::MotorNames::LEFT, mock_rl_motor_driver); + mock_rear_driver->AddMotorDriver( + husarion_ugv_hardware_interfaces::MotorNames::RIGHT, mock_rr_motor_driver); + } + + void DefineDrivers() override + { + drivers_.emplace(husarion_ugv_hardware_interfaces::DriverNames::FRONT, mock_front_driver); + drivers_.emplace(husarion_ugv_hardware_interfaces::DriverNames::REAR, mock_rear_driver); + } + + std::shared_ptr<::testing::NiceMock> + mock_front_driver; + std::shared_ptr<::testing::NiceMock> + mock_rear_driver; + std::shared_ptr<::testing::NiceMock> + mock_fl_motor_driver; + std::shared_ptr<::testing::NiceMock> + mock_fr_motor_driver; + std::shared_ptr<::testing::NiceMock> + mock_rl_motor_driver; + std::shared_ptr<::testing::NiceMock> + mock_rr_motor_driver; +}; + +class TestPantherRobotDriver : public ::testing::Test +{ +public: + TestPantherRobotDriver() + { + can_socket_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name); + can_socket_->Initialize(); + + robot_driver_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings, + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10)); + + robot_driver_->Initialize(); + robot_driver_->Activate(); + } + + ~TestPantherRobotDriver() { robot_driver_->Deinitialize(); } + +protected: + std::unique_ptr can_socket_; + std::unique_ptr robot_driver_; +}; + +TEST_F(TestPantherRobotDriver, SendSpeedCommands) +{ + using husarion_ugv_hardware_interfaces_test::kRadPerSecToRbtqCmd; + + const float fl_v = 0.1; + const float fr_v = 0.2; + const float rl_v = 0.3; + const float rr_v = 0.4; + + EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).Times(1); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).Times(1); + EXPECT_CALL( + *robot_driver_->mock_fl_motor_driver, + SendCmdVel(::testing::Eq(static_cast(fl_v * kRadPerSecToRbtqCmd)))) + .Times(1); + EXPECT_CALL( + *robot_driver_->mock_fr_motor_driver, + SendCmdVel(::testing::Eq(static_cast(fr_v * kRadPerSecToRbtqCmd)))) + .Times(1); + EXPECT_CALL( + *robot_driver_->mock_rl_motor_driver, + SendCmdVel(::testing::Eq(static_cast(rl_v * kRadPerSecToRbtqCmd)))) + .Times(1); + EXPECT_CALL( + *robot_driver_->mock_rr_motor_driver, + SendCmdVel(::testing::Eq(static_cast(rr_v * kRadPerSecToRbtqCmd)))) + .Times(1); + + const std::vector speeds = {fl_v, fr_v, rl_v, rr_v}; + EXPECT_NO_THROW(robot_driver_->SendSpeedCommands(speeds)); +} + +TEST_F(TestPantherRobotDriver, SendSpeedCommandsSendCmdVelError) +{ + EXPECT_CALL(*robot_driver_->mock_fl_motor_driver, SendCmdVel(::testing::_)) + .WillOnce(::testing::Throw(std::runtime_error(""))); + + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0, 0.0, 0.0}); }, + "Front driver send Roboteq cmd failed:")); +} + +TEST_F(TestPantherRobotDriver, SendSpeedCommandsCANError) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(true)); + + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { robot_driver_->SendSpeedCommands({0.0, 0.0, 0.0, 0.0}); }, + "CAN error detected on the front driver when trying to write speed commands.")); +} + +TEST_F(TestPantherRobotDriver, SendSpeedCommandsInvalidVectorSize) +{ + const std::vector speeds = {0.1, 0.2, 0.3}; + + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { robot_driver_->SendSpeedCommands(speeds); }, "Invalid speeds vector size")); +} + +TEST_F(TestPantherRobotDriver, AttemptErrorFlagReset) +{ + EXPECT_NO_THROW(robot_driver_->AttemptErrorFlagReset()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_data_converters.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_data_converters.cpp similarity index 72% rename from panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_data_converters.cpp rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_data_converters.cpp index c7018a1f..6eecd75e 100644 --- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_data_converters.cpp +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_data_converters.cpp @@ -19,7 +19,8 @@ #include -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_data_converters.hpp" + #include "utils/test_constants.hpp" void TestFaultFlagMsg( @@ -69,8 +70,8 @@ void TestRuntimeErrorMsg( TEST(TestRoboteqDataConverters, CommandConverter) { - panther_hardware_interfaces::RoboteqVelocityCommandConverter cmd_converter( - panther_hardware_interfaces_test::kDrivetrainSettings); + husarion_ugv_hardware_interfaces::RoboteqVelocityCommandConverter cmd_converter( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); const float conversion_factor = 79.789678; @@ -83,41 +84,41 @@ TEST(TestRoboteqDataConverters, CommandConverter) TEST(TestRoboteqDataConverters, MotorState) { - panther_hardware_interfaces::MotorState motor_state( - panther_hardware_interfaces_test::kDrivetrainSettings); + using husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + using husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad; + using husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + + husarion_ugv_hardware_interfaces::MotorState motor_state( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); EXPECT_FLOAT_EQ(motor_state.GetPosition(), 0.0); EXPECT_FLOAT_EQ(motor_state.GetVelocity(), 0.0); EXPECT_FLOAT_EQ(motor_state.GetTorque(), 0.0); - panther_hardware_interfaces::RoboteqMotorState feedback1; + husarion_ugv_hardware_interfaces::MotorDriverState feedback1; feedback1.pos = 48128; feedback1.vel = 1000; feedback1.current = 1; motor_state.SetData(feedback1); - const float pos_to_radians = 0.00013055156; - const float vel_to_radians_per_second = 0.003481375; - const float current_to_newton_meters = 0.24816; + EXPECT_FLOAT_EQ(motor_state.GetPosition(), feedback1.pos * kRbtqPosFbToRad); + EXPECT_FLOAT_EQ(motor_state.GetVelocity(), feedback1.vel * kRbtqVelFbToRadPerSec); + EXPECT_FLOAT_EQ(motor_state.GetTorque(), feedback1.current * kRbtqCurrentFbToNewtonMeters); - EXPECT_FLOAT_EQ(motor_state.GetPosition(), feedback1.pos * pos_to_radians); - EXPECT_FLOAT_EQ(motor_state.GetVelocity(), feedback1.vel * vel_to_radians_per_second); - EXPECT_FLOAT_EQ(motor_state.GetTorque(), feedback1.current * current_to_newton_meters); - - panther_hardware_interfaces::RoboteqMotorState feedback2; + husarion_ugv_hardware_interfaces::MotorDriverState feedback2; feedback2.pos = -48128; feedback2.vel = -1000; feedback2.current = -1; motor_state.SetData(feedback2); - EXPECT_FLOAT_EQ(motor_state.GetPosition(), feedback2.pos * pos_to_radians); - EXPECT_FLOAT_EQ(motor_state.GetVelocity(), feedback2.vel * vel_to_radians_per_second); - EXPECT_FLOAT_EQ(motor_state.GetTorque(), feedback2.current * current_to_newton_meters); + EXPECT_FLOAT_EQ(motor_state.GetPosition(), feedback2.pos * kRbtqPosFbToRad); + EXPECT_FLOAT_EQ(motor_state.GetVelocity(), feedback2.vel * kRbtqVelFbToRadPerSec); + EXPECT_FLOAT_EQ(motor_state.GetTorque(), feedback2.current * kRbtqCurrentFbToNewtonMeters); } TEST(TestRoboteqDataConverters, FlagError) { - panther_hardware_interfaces::FlagError flag_error( + husarion_ugv_hardware_interfaces::FlagError flag_error( {"error1", "error2", "error3", "error4", "error5", "error6", "error7", "error8"}, {"error2", "error6"}); @@ -139,7 +140,7 @@ TEST(TestRoboteqDataConverters, FlagError) TEST(TestRoboteqDataConverters, FaultFlag) { - panther_hardware_interfaces::FaultFlag fault_flag; + husarion_ugv_hardware_interfaces::FaultFlag fault_flag; fault_flag.SetData(0b00000001); TestFaultFlagMsg( @@ -172,7 +173,7 @@ TEST(TestRoboteqDataConverters, FaultFlag) TEST(TestRoboteqDataConverters, ScriptFlag) { - panther_hardware_interfaces::ScriptFlag script_flag; + husarion_ugv_hardware_interfaces::ScriptFlag script_flag; script_flag.SetData(0b00000001); TestScriptFlagMsg(script_flag.GetMessage(), {true, false, false}); @@ -187,7 +188,7 @@ TEST(TestRoboteqDataConverters, ScriptFlag) TEST(TestRoboteqDataConverters, RuntimeError) { - panther_hardware_interfaces::RuntimeError runtime_error; + husarion_ugv_hardware_interfaces::RuntimeError runtime_error; runtime_error.SetData(0b00000001); TestRuntimeErrorMsg(runtime_error.GetMessage(), {true, false, false, false, false, false, false}); @@ -214,7 +215,7 @@ TEST(TestRoboteqDataConverters, RuntimeError) TEST(TestRoboteqDataConverters, DriverState) { - panther_hardware_interfaces::DriverState driver_state; + husarion_ugv_hardware_interfaces::RoboteqDriverState driver_state; EXPECT_FLOAT_EQ(driver_state.GetTemperature(), 0.0); EXPECT_FLOAT_EQ(driver_state.GetVoltage(), 0.0); @@ -230,21 +231,29 @@ TEST(TestRoboteqDataConverters, DriverState) EXPECT_FLOAT_EQ(driver_state.GetCurrent(), 3.5); } -TEST(TestRoboteqDataConverters, RoboteqData) +TEST(TestRoboteqDataConverters, DriverData) { - panther_hardware_interfaces::RoboteqData roboteq_data( - panther_hardware_interfaces_test::kDrivetrainSettings); + using husarion_ugv_hardware_interfaces::RoboteqDriver; + using husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + using husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad; + using husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + + husarion_ugv_hardware_interfaces::DriverData roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); ASSERT_FALSE(roboteq_data.IsError()); - const panther_hardware_interfaces::RoboteqMotorState left_state = {48128, 1000, 1}; - const panther_hardware_interfaces::RoboteqMotorState right_state = {0, 0, 0}; - roboteq_data.SetMotorsStates(left_state, right_state, true); + const husarion_ugv_hardware_interfaces::MotorDriverState channel_1_state = { + 0, 0, 0, {0, 0}, {0, 0}}; + const husarion_ugv_hardware_interfaces::MotorDriverState channel_2_state = { + 48128, 1000, 1, {0, 0}, {0, 0}}; + + roboteq_data.SetMotorsStates(channel_1_state, channel_2_state, true); ASSERT_TRUE(roboteq_data.IsError()); ASSERT_TRUE(roboteq_data.IsMotorStatesDataTimedOut()); - roboteq_data.SetMotorsStates(left_state, right_state, false); + roboteq_data.SetMotorsStates(channel_1_state, channel_2_state, false); ASSERT_FALSE(roboteq_data.IsError()); @@ -256,25 +265,25 @@ TEST(TestRoboteqDataConverters, RoboteqData) roboteq_data.SetCANError(false); ASSERT_FALSE(roboteq_data.IsError()); - const float pos_to_radians = 0.00013055156; - const float vel_to_radians_per_second = 0.003481375; - const float current_to_newton_meters = 0.24816; - - EXPECT_FLOAT_EQ(roboteq_data.GetRightMotorState().GetPosition(), 0.0); - EXPECT_FLOAT_EQ(roboteq_data.GetRightMotorState().GetVelocity(), 0.0); - EXPECT_FLOAT_EQ(roboteq_data.GetRightMotorState().GetTorque(), 0.0); - EXPECT_FLOAT_EQ(roboteq_data.GetLeftMotorState().GetPosition(), left_state.pos * pos_to_radians); + EXPECT_FLOAT_EQ(roboteq_data.GetMotorState(RoboteqDriver::kChannel1).GetPosition(), 0.0); + EXPECT_FLOAT_EQ(roboteq_data.GetMotorState(RoboteqDriver::kChannel1).GetVelocity(), 0.0); + EXPECT_FLOAT_EQ(roboteq_data.GetMotorState(RoboteqDriver::kChannel1).GetTorque(), 0.0); + EXPECT_FLOAT_EQ( + roboteq_data.GetMotorState(RoboteqDriver::kChannel2).GetPosition(), + channel_2_state.pos * kRbtqPosFbToRad); EXPECT_FLOAT_EQ( - roboteq_data.GetLeftMotorState().GetVelocity(), left_state.vel * vel_to_radians_per_second); + roboteq_data.GetMotorState(RoboteqDriver::kChannel2).GetVelocity(), + channel_2_state.vel * kRbtqVelFbToRadPerSec); EXPECT_FLOAT_EQ( - roboteq_data.GetLeftMotorState().GetTorque(), left_state.current * current_to_newton_meters); + roboteq_data.GetMotorState(RoboteqDriver::kChannel2).GetTorque(), + channel_2_state.current * kRbtqCurrentFbToNewtonMeters); - panther_hardware_interfaces::RoboteqDriverState state; + husarion_ugv_hardware_interfaces::DriverState state; state.fault_flags = 0b00000001; state.script_flags = 0b00000010; - state.runtime_stat_flag_motor_1 = 0b00010000; - state.runtime_stat_flag_motor_2 = 0b00000100; + state.runtime_stat_flag_channel_1 = 0b00010000; + state.runtime_stat_flag_channel_2 = 0b00000100; state.mcu_temp = 32; state.heatsink_temp = 31; @@ -288,8 +297,9 @@ TEST(TestRoboteqDataConverters, RoboteqData) EXPECT_TRUE(roboteq_data.GetFaultFlag().GetMessage().overheat); EXPECT_TRUE(roboteq_data.GetScriptFlag().GetMessage().encoder_disconnected); - EXPECT_TRUE(roboteq_data.GetLeftRuntimeError().GetMessage().loop_error); - EXPECT_TRUE(roboteq_data.GetRightRuntimeError().GetMessage().forward_limit_triggered); + EXPECT_TRUE(roboteq_data.GetRuntimeError(RoboteqDriver::kChannel2).GetMessage().loop_error); + EXPECT_TRUE( + roboteq_data.GetRuntimeError(RoboteqDriver::kChannel1).GetMessage().forward_limit_triggered); EXPECT_FLOAT_EQ(roboteq_data.GetDriverState().GetTemperature(), 32); EXPECT_FLOAT_EQ(roboteq_data.GetDriverState().GetHeatsinkTemperature(), 31); diff --git a/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_driver.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_driver.cpp new file mode 100644 index 00000000..9468ab79 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_driver.cpp @@ -0,0 +1,390 @@ +// Copyright 2023 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 "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_driver.hpp" + +#include "utils/fake_can_socket.hpp" +#include "utils/mock_roboteq.hpp" +#include "utils/test_constants.hpp" + +#include "husarion_ugv_utils/test/test_utils.hpp" + +class TestRoboteqDriver : public ::testing::Test +{ +public: + TestRoboteqDriver(); + + ~TestRoboteqDriver(); + + void BootRoboteqDriver(); + +protected: + bool TestBoot(); + + static constexpr char kMotor1Name[] = "motor_1"; + static constexpr char kMotor2Name[] = "motor_2"; + + std::unique_ptr can_socket_; + std::unique_ptr mock_roboteq_; + std::unique_ptr canopen_manager_; + std::shared_ptr roboteq_driver_; +}; + +TestRoboteqDriver::TestRoboteqDriver() +{ + can_socket_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name); + can_socket_->Initialize(); + + canopen_manager_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings); + + mock_roboteq_ = std::make_unique(); + mock_roboteq_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50)); + canopen_manager_->Initialize(); + + roboteq_driver_ = std::make_shared( + canopen_manager_->GetMaster(), 1, std::chrono::milliseconds(100)); + + canopen_manager_->Activate(); +} + +TestRoboteqDriver::~TestRoboteqDriver() +{ + roboteq_driver_.reset(); + canopen_manager_->Deinitialize(); + mock_roboteq_->Stop(); + mock_roboteq_.reset(); + can_socket_->Deinitialize(); +} + +void TestRoboteqDriver::BootRoboteqDriver() +{ + auto motor_1 = std::make_shared( + roboteq_driver_, husarion_ugv_hardware_interfaces::RoboteqDriver::kChannel1); + auto motor_2 = std::make_shared( + roboteq_driver_, husarion_ugv_hardware_interfaces::RoboteqDriver::kChannel2); + + roboteq_driver_->AddMotorDriver(kMotor1Name, motor_1); + roboteq_driver_->AddMotorDriver(kMotor2Name, motor_2); + auto future = roboteq_driver_->Boot(); + future.wait(); +} + +bool TestRoboteqDriver::TestBoot() +{ + auto future = roboteq_driver_->Boot(); + const auto future_status = future.wait_for(std::chrono::milliseconds(500)); + + if (future_status != std::future_status::ready) { + return false; + } + + try { + future.get(); + } catch (const std::exception & e) { + return false; + } + + return true; +} + +TEST_F(TestRoboteqDriver, BootRoboteqDriver) { ASSERT_TRUE(TestBoot()); } + +TEST_F(TestRoboteqDriver, BootErrorDeviceType) +{ + const auto device_type_id = 0x1000; + const auto device_type_subid = 0; + mock_roboteq_->GetDriver()->SetOnReadWait( + device_type_id, device_type_subid, 100000); + EXPECT_FALSE(TestBoot()); +} + +TEST_F(TestRoboteqDriver, BootErrorVendorId) +{ + const auto vendor_id_id = 0x1018; + const auto vendor_id_subid = 1; + mock_roboteq_->GetDriver()->SetOnReadWait(vendor_id_id, vendor_id_subid, 100000); + EXPECT_FALSE(TestBoot()); +} + +TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates) +{ + using husarion_ugv_hardware_interfaces_test::DriverChannel; + + const std::int32_t motor_1_pos = 101; + const std::int32_t motor_1_vel = 102; + const std::int32_t motor_1_current = 103; + const std::int32_t motor_2_pos = 201; + const std::int32_t motor_2_vel = 202; + const std::int32_t motor_2_current = 203; + + BootRoboteqDriver(); + + mock_roboteq_->GetDriver()->SetPosition(DriverChannel::CHANNEL1, motor_1_pos); + mock_roboteq_->GetDriver()->SetPosition(DriverChannel::CHANNEL2, motor_2_pos); + + mock_roboteq_->GetDriver()->SetVelocity(DriverChannel::CHANNEL1, motor_1_vel); + mock_roboteq_->GetDriver()->SetVelocity(DriverChannel::CHANNEL2, motor_2_vel); + + mock_roboteq_->GetDriver()->SetCurrent(DriverChannel::CHANNEL1, motor_1_current); + mock_roboteq_->GetDriver()->SetCurrent(DriverChannel::CHANNEL2, motor_2_current); + + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state_1 = + roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadState(); + husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state_2 = + roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadState(); + + EXPECT_EQ(motor_driver_state_1.pos, motor_1_pos); + EXPECT_EQ(motor_driver_state_1.vel, motor_1_vel); + EXPECT_EQ(motor_driver_state_1.current, motor_1_current); + + EXPECT_EQ(motor_driver_state_2.pos, motor_2_pos); + EXPECT_EQ(motor_driver_state_2.vel, motor_2_vel); + EXPECT_EQ(motor_driver_state_2.current, motor_2_current); +} + +TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps) +{ + BootRoboteqDriver(); + + husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state_1 = + roboteq_driver_->GetMotorDriver(kMotor1Name)->ReadState(); + + // based on publishing frequency in the Roboteq mock (100Hz) + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state_2 = + roboteq_driver_->GetMotorDriver(kMotor2Name)->ReadState(); + + // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked + // if consecutive messages will have timestamps 100ms + some threshold apart + EXPECT_LE( + lely::util::from_timespec(motor_driver_state_2.pos_timestamp) - + lely::util::from_timespec(motor_driver_state_1.pos_timestamp), + std::chrono::milliseconds(15)); + + EXPECT_GE( + lely::util::from_timespec(motor_driver_state_2.pos_timestamp) - + lely::util::from_timespec(motor_driver_state_1.pos_timestamp), + std::chrono::milliseconds(5)); + + EXPECT_LE( + lely::util::from_timespec(motor_driver_state_2.vel_current_timestamp) - + lely::util::from_timespec(motor_driver_state_1.vel_current_timestamp), + std::chrono::milliseconds(15)); + + EXPECT_GE( + lely::util::from_timespec(motor_driver_state_2.vel_current_timestamp) - + lely::util::from_timespec(motor_driver_state_1.vel_current_timestamp), + std::chrono::milliseconds(5)); +} + +TEST_F(TestRoboteqDriver, ReadState) +{ + using husarion_ugv_hardware_interfaces_test::DriverChannel; + using husarion_ugv_hardware_interfaces_test::DriverFaultFlags; + using husarion_ugv_hardware_interfaces_test::DriverRuntimeErrors; + using husarion_ugv_hardware_interfaces_test::DriverScriptFlags; + + const std::int16_t temp = 30; + const std::int16_t heatsink_temp = 31; + const std::uint16_t volt = 400; + const std::int16_t battery_current_1 = 10; + const std::int16_t battery_current_2 = 30; + + mock_roboteq_->GetDriver()->SetTemperature(temp); + mock_roboteq_->GetDriver()->SetHeatsinkTemperature(heatsink_temp); + mock_roboteq_->GetDriver()->SetVoltage(volt); + mock_roboteq_->GetDriver()->SetBatteryCurrent1(battery_current_1); + mock_roboteq_->GetDriver()->SetBatteryCurrent2(battery_current_2); + + mock_roboteq_->GetDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT); + mock_roboteq_->GetDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED); + mock_roboteq_->GetDriver()->SetDriverRuntimeError( + DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR); + mock_roboteq_->GetDriver()->SetDriverRuntimeError( + DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + husarion_ugv_hardware_interfaces::DriverState driver_state = roboteq_driver_->ReadState(); + + EXPECT_EQ(driver_state.mcu_temp, temp); + EXPECT_EQ(driver_state.heatsink_temp, heatsink_temp); + EXPECT_EQ(driver_state.battery_voltage, volt); + EXPECT_EQ(driver_state.battery_current_1, battery_current_1); + EXPECT_EQ(driver_state.battery_current_2, battery_current_2); + + EXPECT_EQ(driver_state.fault_flags, 0b00000001); + EXPECT_EQ(driver_state.script_flags, 0b00000010); + EXPECT_EQ(driver_state.runtime_stat_flag_channel_1, 0b00000100); + EXPECT_EQ(driver_state.runtime_stat_flag_channel_2, 0b00001000); +} + +TEST_F(TestRoboteqDriver, ReadStateTimestamp) +{ + BootRoboteqDriver(); + + husarion_ugv_hardware_interfaces::DriverState driver_state_1 = roboteq_driver_->ReadState(); + + // based on publishing frequency in the Roboteq mock (20Hz) + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + husarion_ugv_hardware_interfaces::DriverState driver_state_2 = roboteq_driver_->ReadState(); + + // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked + // if consecutive messages will have timestamps 100ms + some threshold apart + EXPECT_LE( + lely::util::from_timespec(driver_state_2.flags_current_timestamp) - + lely::util::from_timespec(driver_state_1.flags_current_timestamp), + std::chrono::milliseconds(75)); + + EXPECT_GE( + lely::util::from_timespec(driver_state_2.flags_current_timestamp) - + lely::util::from_timespec(driver_state_1.flags_current_timestamp), + std::chrono::milliseconds(25)); + + EXPECT_LE( + lely::util::from_timespec(driver_state_2.voltages_temps_timestamp) - + lely::util::from_timespec(driver_state_1.voltages_temps_timestamp), + std::chrono::milliseconds(75)); + + EXPECT_GE( + lely::util::from_timespec(driver_state_2.voltages_temps_timestamp) - + lely::util::from_timespec(driver_state_1.voltages_temps_timestamp), + std::chrono::milliseconds(25)); +} + +TEST_F(TestRoboteqDriver, SendRoboteqCmd) +{ + using husarion_ugv_hardware_interfaces_test::DriverChannel; + + const std::int32_t motor_1_v = 10; + const std::int32_t motor_2_v = 20; + + BootRoboteqDriver(); + + roboteq_driver_->GetMotorDriver(kMotor1Name)->SendCmdVel(motor_1_v); + roboteq_driver_->GetMotorDriver(kMotor2Name)->SendCmdVel(motor_2_v); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + EXPECT_EQ(mock_roboteq_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), motor_1_v); + EXPECT_EQ(mock_roboteq_->GetDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), motor_2_v); +} + +TEST_F(TestRoboteqDriver, ResetRoboteqScript) +{ + BootRoboteqDriver(); + mock_roboteq_->GetDriver()->SetResetRoboteqScript(65); + roboteq_driver_->ResetScript(); + + EXPECT_EQ(mock_roboteq_->GetDriver()->GetResetRoboteqScript(), 2); +} + +TEST_F(TestRoboteqDriver, ResetRoboteqScriptSDOTimeoutReset) +{ + BootRoboteqDriver(); + + mock_roboteq_->GetDriver()->SetOnWriteWait(0x2018, 0, 100000); + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { roboteq_driver_->ResetScript(); }, "SDO protocol timed out")); + + mock_roboteq_->GetDriver()->SetOnWriteWait(0x2018, 0, 0); + EXPECT_NO_THROW(roboteq_driver_->ResetScript()); +} + +TEST_F(TestRoboteqDriver, RoboteqTurnOnEStop) +{ + BootRoboteqDriver(); + mock_roboteq_->GetDriver()->SetTurnOnEStop(65); + roboteq_driver_->TurnOnEStop(); + + EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnEStop(), 1); +} + +TEST_F(TestRoboteqDriver, TurnOnEStopTimeout) +{ + BootRoboteqDriver(); + mock_roboteq_->GetDriver()->SetOnWriteWait(0x200C, 0, 100000); + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { roboteq_driver_->TurnOnEStop(); }, "SDO protocol timed out")); +} + +TEST_F(TestRoboteqDriver, TurnOffEStop) +{ + BootRoboteqDriver(); + mock_roboteq_->GetDriver()->SetTurnOffEStop(65); + roboteq_driver_->TurnOffEStop(); + + EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOffEStop(), 1); +} + +TEST_F(TestRoboteqDriver, TurnOffEStopTimeout) +{ + BootRoboteqDriver(); + mock_roboteq_->GetDriver()->SetOnWriteWait(0x200D, 0, 100000); + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { roboteq_driver_->TurnOffEStop(); }, "SDO protocol timed out")); +} + +TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1) +{ + BootRoboteqDriver(); + mock_roboteq_->GetDriver()->SetTurnOnSafetyStop(67); + roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(); + + EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnSafetyStop(), 1); +} + +TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2) +{ + BootRoboteqDriver(); + mock_roboteq_->GetDriver()->SetTurnOnSafetyStop(65); + roboteq_driver_->GetMotorDriver(kMotor2Name)->TurnOnSafetyStop(); + + EXPECT_EQ(mock_roboteq_->GetDriver()->GetTurnOnSafetyStop(), 2); +} + +TEST_F(TestRoboteqDriver, WriteTimeout) +{ + BootRoboteqDriver(); + mock_roboteq_->GetDriver()->SetOnWriteWait(0x202C, 0, 200000); + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { roboteq_driver_->GetMotorDriver(kMotor1Name)->TurnOnSafetyStop(); }, + "SDO protocol timed out")); +} + +// OnCanError/OnHeartbeatTimeout isn't tested, because it reacts to lower-level CAN errors (CRC), +// which are hard to simulate, but it would be nice to add it + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + auto result = RUN_ALL_TESTS(); + return result; +} diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_error_filter.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_error_filter.cpp similarity index 87% rename from panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_error_filter.cpp rename to husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_error_filter.cpp index 999243f6..b4fe4ede 100644 --- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_error_filter.cpp +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_error_filter.cpp @@ -14,13 +14,13 @@ #include -#include +#include TEST(TestRoboteqErrorFilter, InitialState) { - using panther_hardware_interfaces::ErrorsFilterIds; + using husarion_ugv_hardware_interfaces::ErrorsFilterIds; - panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); + husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); ASSERT_FALSE(roboteq_error_filter.IsError()); EXPECT_FALSE(roboteq_error_filter.IsError(ErrorsFilterIds::WRITE_PDO_CMDS)); @@ -31,9 +31,9 @@ TEST(TestRoboteqErrorFilter, InitialState) TEST(TestRoboteqErrorFilter, FilterError) { - using panther_hardware_interfaces::ErrorsFilterIds; + using husarion_ugv_hardware_interfaces::ErrorsFilterIds; - panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); + husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true); @@ -62,9 +62,9 @@ TEST(TestRoboteqErrorFilter, FilterError) TEST(TestRoboteqErrorFilter, Error) { - using panther_hardware_interfaces::ErrorsFilterIds; + using husarion_ugv_hardware_interfaces::ErrorsFilterIds; - panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); + husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true); @@ -85,9 +85,9 @@ TEST(TestRoboteqErrorFilter, Error) TEST(TestRoboteqErrorFilter, FilterSecondError) { - using panther_hardware_interfaces::ErrorsFilterIds; + using husarion_ugv_hardware_interfaces::ErrorsFilterIds; - panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); + husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); roboteq_error_filter.UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); @@ -116,9 +116,9 @@ TEST(TestRoboteqErrorFilter, FilterSecondError) TEST(TestRoboteqErrorFilter, SecondError) { - using panther_hardware_interfaces::ErrorsFilterIds; + using husarion_ugv_hardware_interfaces::ErrorsFilterIds; - panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); + husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); roboteq_error_filter.UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); @@ -139,9 +139,9 @@ TEST(TestRoboteqErrorFilter, SecondError) TEST(TestRoboteqErrorFilter, ErrorSingle) { - using panther_hardware_interfaces::ErrorsFilterIds; + using husarion_ugv_hardware_interfaces::ErrorsFilterIds; - panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); + husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); roboteq_error_filter.UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); @@ -154,9 +154,9 @@ TEST(TestRoboteqErrorFilter, ErrorSingle) TEST(TestRoboteqErrorFilter, ClearErrors) { - using panther_hardware_interfaces::ErrorsFilterIds; + using husarion_ugv_hardware_interfaces::ErrorsFilterIds; - panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); + husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true); roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true); @@ -187,9 +187,9 @@ TEST(TestRoboteqErrorFilter, ClearErrors) TEST(TestRoboteqErrorFilter, ClearErrorsCounters) { - using panther_hardware_interfaces::ErrorsFilterIds; + using husarion_ugv_hardware_interfaces::ErrorsFilterIds; - panther_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); + husarion_ugv_hardware_interfaces::RoboteqErrorFilter roboteq_error_filter(2, 2, 1, 1); roboteq_error_filter.UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true); roboteq_error_filter.UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); diff --git a/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_robot_driver.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_robot_driver.cpp new file mode 100644 index 00000000..26ee9120 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/robot_driver/test_roboteq_robot_driver.cpp @@ -0,0 +1,538 @@ +// Copyright 2023 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 + +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/canopen_manager.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp" + +#include "utils/fake_can_socket.hpp" +#include "utils/mock_driver.hpp" +#include "utils/test_constants.hpp" + +#include "husarion_ugv_utils/test/test_utils.hpp" + +class RoboteqRobotDriverWrapper : public husarion_ugv_hardware_interfaces::RoboteqRobotDriver +{ +public: + RoboteqRobotDriverWrapper( + const husarion_ugv_hardware_interfaces::CANopenSettings & canopen_settings, + const husarion_ugv_hardware_interfaces::DrivetrainSettings & drivetrain_settings, + const std::chrono::milliseconds activate_wait_time = std::chrono::milliseconds(1000)) + : RoboteqRobotDriver(canopen_settings, drivetrain_settings, activate_wait_time) + { + // Assume 2 drivers and 4 motor drivers + mock_fl_motor_driver = + std::make_shared(); + mock_fr_motor_driver = + std::make_shared(); + mock_rl_motor_driver = + std::make_shared(); + mock_rr_motor_driver = + std::make_shared(); + + mock_front_driver = + std::make_shared(); + mock_front_driver->AddMotorDriver(kLeftMotorDriverName, mock_fl_motor_driver); + mock_front_driver->AddMotorDriver(kRightMotorDriverName, mock_fr_motor_driver); + + mock_rear_driver = + std::make_shared(); + mock_rear_driver->AddMotorDriver(kLeftMotorDriverName, mock_rl_motor_driver); + mock_rear_driver->AddMotorDriver(kRightMotorDriverName, mock_rr_motor_driver); + } + + void DefineDrivers() override + { + drivers_.emplace(kFrontDriverName, mock_front_driver); + drivers_.emplace(kRearDriverName, mock_rear_driver); + } + + void SendSpeedCommands(const std::vector & /*velocities*/) override {} + void AttemptErrorFlagReset() override {} + + static constexpr char kFrontDriverName[] = "front"; + static constexpr char kRearDriverName[] = "rear"; + static constexpr char kLeftMotorDriverName[] = "left"; + static constexpr char kRightMotorDriverName[] = "right"; + + std::shared_ptr mock_front_driver; + std::shared_ptr mock_rear_driver; + std::shared_ptr + mock_fl_motor_driver; + std::shared_ptr + mock_fr_motor_driver; + std::shared_ptr + mock_rl_motor_driver; + std::shared_ptr + mock_rr_motor_driver; +}; + +class TestRoboteqRobotDriverInitialization : public ::testing::Test +{ +public: + TestRoboteqRobotDriverInitialization() + { + can_socket_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings.can_interface_name); + can_socket_->Initialize(); + + robot_driver_ = std::make_unique( + husarion_ugv_hardware_interfaces_test::kCANopenSettings, + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings, std::chrono::milliseconds(10)); + } + + ~TestRoboteqRobotDriverInitialization() {} + +protected: + std::unique_ptr can_socket_; + std::unique_ptr robot_driver_; +}; + +class TestRoboteqRobotDriver : public TestRoboteqRobotDriverInitialization +{ +public: + TestRoboteqRobotDriver() + { + robot_driver_->Initialize(); + robot_driver_->Activate(); + } + + ~TestRoboteqRobotDriver() { robot_driver_->Deinitialize(); } + + timespec GetCurrentTimeWithTimeout(const std::chrono::nanoseconds & timeout_ns) + { + timespec current_time; + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + + current_time.tv_nsec -= timeout_ns.count(); + + return current_time; + } +}; + +TEST_F(TestRoboteqRobotDriverInitialization, Initialize) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, Boot()).Times(1); + EXPECT_CALL(*robot_driver_->mock_rear_driver, Boot()).Times(1); + + EXPECT_NO_THROW(robot_driver_->Initialize()); + ASSERT_NO_THROW(robot_driver_->Deinitialize()); + + EXPECT_CALL(*robot_driver_->mock_front_driver, Boot()).Times(1); + EXPECT_CALL(*robot_driver_->mock_rear_driver, Boot()).Times(1); + // Check if deinitialization worked correctly - initialize once again + EXPECT_NO_THROW(robot_driver_->Initialize()); +} + +TEST_F(TestRoboteqRobotDriverInitialization, Activate) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, ResetScript()).Times(1); + EXPECT_CALL(*robot_driver_->mock_rear_driver, ResetScript()).Times(1); + EXPECT_CALL(*robot_driver_->mock_fl_motor_driver, SendCmdVel(::testing::Eq(0))).Times(1); + EXPECT_CALL(*robot_driver_->mock_fr_motor_driver, SendCmdVel(::testing::Eq(0))).Times(1); + EXPECT_CALL(*robot_driver_->mock_rl_motor_driver, SendCmdVel(::testing::Eq(0))).Times(1); + EXPECT_CALL(*robot_driver_->mock_rr_motor_driver, SendCmdVel(::testing::Eq(0))).Times(1); + + ASSERT_NO_THROW(robot_driver_->Initialize()); + ASSERT_NO_THROW(robot_driver_->Activate()); +} + +TEST_F(TestRoboteqRobotDriver, GetData) +{ + EXPECT_NO_THROW(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName)); + EXPECT_NO_THROW(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName)); +} + +TEST_F(TestRoboteqRobotDriver, GetDataError) +{ + const std::string name = "invalid_name"; + const std::string error_msg = "Data with name '" + name + "' does not exist."; + + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&] { robot_driver_->GetData(name); }, error_msg)); +} + +TEST_F(TestRoboteqRobotDriver, UpdateCommunicationState) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).Times(1); + EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout()).Times(1); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).Times(1); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout()).Times(1); + + ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState()); +} + +TEST_F(TestRoboteqRobotDriver, UpdateCommunicationStateCANErorr) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(true)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).WillOnce(::testing::Return(true)); + + ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState()); + + EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).IsCANError()); + EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsCANError()); +} + +TEST_F(TestRoboteqRobotDriver, UpdateCommunicationStateHeartbeatTimeout) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(true)); + + ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState()); + + EXPECT_TRUE( + robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).IsHeartbeatTimeout()); + EXPECT_TRUE( + robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsHeartbeatTimeout()); +} + +TEST_F(TestRoboteqRobotDriver, UpdateMotorsState) +{ + using husarion_ugv_hardware_interfaces::MotorChannels; + using husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + using husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad; + using husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + + const std::int32_t fl_pos = 101; + const std::int32_t fl_vel = 102; + const std::int32_t fl_current = 103; + const std::int32_t fr_pos = 201; + const std::int32_t fr_vel = 202; + const std::int32_t fr_current = 203; + const std::int32_t rl_pos = 301; + const std::int32_t rl_vel = 302; + const std::int32_t rl_current = 303; + const std::int32_t rr_pos = 401; + const std::int32_t rr_vel = 402; + const std::int32_t rr_current = 403; + + ON_CALL(*robot_driver_->mock_fl_motor_driver, ReadState()) + .WillByDefault(::testing::Return(husarion_ugv_hardware_interfaces::MotorDriverState( + {fl_pos, fl_vel, fl_current, {0, 0}, {0, 0}}))); + ON_CALL(*robot_driver_->mock_fr_motor_driver, ReadState()) + .WillByDefault(::testing::Return(husarion_ugv_hardware_interfaces::MotorDriverState( + {fr_pos, fr_vel, fr_current, {0, 0}, {0, 0}}))); + ON_CALL(*robot_driver_->mock_rl_motor_driver, ReadState()) + .WillByDefault(::testing::Return(husarion_ugv_hardware_interfaces::MotorDriverState( + {rl_pos, rl_vel, rl_current, {0, 0}, {0, 0}}))); + ON_CALL(*robot_driver_->mock_rr_motor_driver, ReadState()) + .WillByDefault(::testing::Return(husarion_ugv_hardware_interfaces::MotorDriverState( + {rr_pos, rr_vel, rr_current, {0, 0}, {0, 0}}))); + + robot_driver_->UpdateMotorsState(); + + const auto & fl_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName) + .GetMotorState(MotorChannels::LEFT); + const auto & fr_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName) + .GetMotorState(MotorChannels::RIGHT); + const auto & rl_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName) + .GetMotorState(MotorChannels::LEFT); + const auto & rr_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName) + .GetMotorState(MotorChannels::RIGHT); + + EXPECT_FLOAT_EQ(fl_data.GetPosition(), fl_pos * kRbtqPosFbToRad); + EXPECT_FLOAT_EQ(fl_data.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec); + EXPECT_FLOAT_EQ(fl_data.GetTorque(), fl_current * kRbtqCurrentFbToNewtonMeters); + + EXPECT_FLOAT_EQ(fr_data.GetPosition(), fr_pos * kRbtqPosFbToRad); + EXPECT_FLOAT_EQ(fr_data.GetVelocity(), fr_vel * kRbtqVelFbToRadPerSec); + EXPECT_FLOAT_EQ(fr_data.GetTorque(), fr_current * kRbtqCurrentFbToNewtonMeters); + + EXPECT_FLOAT_EQ(rl_data.GetPosition(), rl_pos * kRbtqPosFbToRad); + EXPECT_FLOAT_EQ(rl_data.GetVelocity(), rl_vel * kRbtqVelFbToRadPerSec); + EXPECT_FLOAT_EQ(rl_data.GetTorque(), rl_current * kRbtqCurrentFbToNewtonMeters); + + EXPECT_FLOAT_EQ(rr_data.GetPosition(), rr_pos * kRbtqPosFbToRad); + EXPECT_FLOAT_EQ(rr_data.GetVelocity(), rr_vel * kRbtqVelFbToRadPerSec); + EXPECT_FLOAT_EQ(rr_data.GetTorque(), rr_current * kRbtqCurrentFbToNewtonMeters); +} + +TEST_F(TestRoboteqRobotDriver, UpdateMotorsStateTimestamps) +{ + auto current_time = GetCurrentTimeWithTimeout( + husarion_ugv_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms); + + auto read_motor_driver_state_method = [¤t_time]() { + husarion_ugv_hardware_interfaces::MotorDriverState state; + state.pos_timestamp = current_time; + state.vel_current_timestamp = current_time; + return state; + }; + + ON_CALL(*robot_driver_->mock_fl_motor_driver, ReadState()) + .WillByDefault(::testing::Invoke(read_motor_driver_state_method)); + ON_CALL(*robot_driver_->mock_fr_motor_driver, ReadState()) + .WillByDefault(::testing::Invoke(read_motor_driver_state_method)); + ON_CALL(*robot_driver_->mock_rl_motor_driver, ReadState()) + .WillByDefault(::testing::Invoke(read_motor_driver_state_method)); + ON_CALL(*robot_driver_->mock_rr_motor_driver, ReadState()) + .WillByDefault(::testing::Invoke(read_motor_driver_state_method)); + + robot_driver_->UpdateMotorsState(); + + // Update current time to exceed timeout + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + + robot_driver_->UpdateMotorsState(); + + EXPECT_FALSE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName) + .IsMotorStatesDataTimedOut()); + EXPECT_FALSE( + robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsMotorStatesDataTimedOut()); +} + +TEST_F(TestRoboteqRobotDriver, UpdateMotorsStateTimeout) +{ + const auto current_time = GetCurrentTimeWithTimeout( + husarion_ugv_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms); + + husarion_ugv_hardware_interfaces::MotorDriverState state = {0, 0, 0, current_time, current_time}; + + ON_CALL(*robot_driver_->mock_fl_motor_driver, ReadState()) + .WillByDefault(::testing::Return(state)); + ON_CALL(*robot_driver_->mock_fr_motor_driver, ReadState()) + .WillByDefault(::testing::Return(state)); + ON_CALL(*robot_driver_->mock_rl_motor_driver, ReadState()) + .WillByDefault(::testing::Return(state)); + ON_CALL(*robot_driver_->mock_rr_motor_driver, ReadState()) + .WillByDefault(::testing::Return(state)); + + robot_driver_->UpdateMotorsState(); + + EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName) + .IsMotorStatesDataTimedOut()); + EXPECT_TRUE( + robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsMotorStatesDataTimedOut()); + EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).IsError()); + EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsError()); +} + +TEST_F(TestRoboteqRobotDriver, UpdateDriverState) +{ + using husarion_ugv_hardware_interfaces::MotorChannels; + + const std::int16_t f_temp = 30; + const std::int16_t r_temp = 32; + const std::int16_t f_heatsink_temp = 31; + const std::int16_t r_heatsink_temp = 33; + const std::uint16_t f_volt = 400; + const std::uint16_t r_volt = 430; + const std::int16_t f_battery_current_1 = 10; + const std::int16_t r_battery_current_1 = 30; + const std::int16_t f_battery_current_2 = 30; + const std::int16_t r_battery_current_2 = 40; + + const std::uint8_t fault_flag_overheat = static_cast(0b01); + const std::uint8_t fault_flag_overvoltage = static_cast(0b10); + const std::uint8_t script_flag_encoder_disconnected = static_cast(0b10); + const std::uint8_t script_flag_amp_limiter = static_cast(0b100); + const std::uint8_t runtime_error_loop_error = static_cast(0b100); + const std::uint8_t runtime_error_safety_stop_active = static_cast(0b1000); + const std::uint8_t runtime_error_forward_limit_triggered = static_cast(0b10000); + const std::uint8_t runtime_error_reverse_limit_triggered = static_cast(0b100000); + + husarion_ugv_hardware_interfaces::DriverState front_driver_state_data = { + fault_flag_overheat, + script_flag_encoder_disconnected, + runtime_error_loop_error, + runtime_error_safety_stop_active, + f_battery_current_1, + f_battery_current_2, + f_volt, + f_temp, + f_heatsink_temp, + {0, 0}, + {0, 0}}; + + husarion_ugv_hardware_interfaces::DriverState rear_driver_state_data = { + fault_flag_overvoltage, + script_flag_amp_limiter, + runtime_error_forward_limit_triggered, + runtime_error_reverse_limit_triggered, + r_battery_current_1, + r_battery_current_2, + r_volt, + r_temp, + r_heatsink_temp, + {0, 0}, + {0, 0}}; + + ON_CALL(*robot_driver_->mock_front_driver, ReadState()) + .WillByDefault( + ::testing::Return(husarion_ugv_hardware_interfaces::DriverState(front_driver_state_data))); + ON_CALL(*robot_driver_->mock_rear_driver, ReadState()) + .WillByDefault( + ::testing::Return(husarion_ugv_hardware_interfaces::DriverState(rear_driver_state_data))); + + robot_driver_->UpdateDriversState(); + + const auto & front_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName); + const auto & rear_data = robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName); + const auto & front_driver_state = + robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).GetDriverState(); + const auto & rear_driver_state = + robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).GetDriverState(); + + EXPECT_EQ(static_cast(front_driver_state.GetTemperature()), f_temp); + EXPECT_EQ( + static_cast(front_driver_state.GetHeatsinkTemperature()), f_heatsink_temp); + EXPECT_EQ(static_cast(front_driver_state.GetVoltage() * 10.0), f_volt); + EXPECT_EQ( + static_cast(front_driver_state.GetCurrent() * 10.0), + f_battery_current_1 + f_battery_current_2); + + EXPECT_EQ(static_cast(rear_driver_state.GetTemperature()), r_temp); + EXPECT_EQ(static_cast(rear_driver_state.GetHeatsinkTemperature()), r_heatsink_temp); + EXPECT_EQ(static_cast(rear_driver_state.GetVoltage() * 10.0), r_volt); + EXPECT_EQ( + static_cast(rear_driver_state.GetCurrent() * 10.0), + r_battery_current_1 + r_battery_current_2); + + EXPECT_TRUE(front_data.GetFaultFlag().GetMessage().overheat); + EXPECT_TRUE(front_data.GetScriptFlag().GetMessage().encoder_disconnected); + EXPECT_TRUE(front_data.GetRuntimeError(MotorChannels::RIGHT).GetMessage().loop_error); + EXPECT_TRUE(front_data.GetRuntimeError(MotorChannels::LEFT).GetMessage().safety_stop_active); + + EXPECT_TRUE(rear_data.GetFaultFlag().GetMessage().overvoltage); + EXPECT_TRUE(rear_data.GetScriptFlag().GetMessage().amp_limiter); + EXPECT_TRUE(rear_data.GetRuntimeError(MotorChannels::RIGHT).GetMessage().forward_limit_triggered); + EXPECT_TRUE(rear_data.GetRuntimeError(MotorChannels::LEFT).GetMessage().reverse_limit_triggered); +} + +TEST_F(TestRoboteqRobotDriver, UpdateDriverStateTimestamps) +{ + auto current_time = GetCurrentTimeWithTimeout( + husarion_ugv_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms); + + auto read_driver_state_method = [¤t_time]() { + husarion_ugv_hardware_interfaces::DriverState state; + state.flags_current_timestamp = current_time; + state.voltages_temps_timestamp = current_time; + return state; + }; + + ON_CALL(*robot_driver_->mock_front_driver, ReadState()) + .WillByDefault(::testing::Invoke(read_driver_state_method)); + ON_CALL(*robot_driver_->mock_rear_driver, ReadState()) + .WillByDefault(::testing::Invoke(read_driver_state_method)); + + robot_driver_->UpdateDriversState(); + + // Update current time to exceed timeout + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + + robot_driver_->UpdateDriversState(); + + EXPECT_FALSE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName) + .IsDriverStateDataTimedOut()); + EXPECT_FALSE( + robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsDriverStateDataTimedOut()); +} + +TEST_F(TestRoboteqRobotDriver, UpdateDriverStateTimeout) +{ + const auto current_time = GetCurrentTimeWithTimeout( + husarion_ugv_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms); + + husarion_ugv_hardware_interfaces::DriverState state = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, current_time, current_time}; + + ON_CALL(*robot_driver_->mock_front_driver, ReadState()).WillByDefault(::testing::Return(state)); + ON_CALL(*robot_driver_->mock_rear_driver, ReadState()).WillByDefault(::testing::Return(state)); + + robot_driver_->UpdateDriversState(); + + EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName) + .IsDriverStateDataTimedOut()); + EXPECT_TRUE( + robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsDriverStateDataTimedOut()); + EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kFrontDriverName).IsError()); + EXPECT_TRUE(robot_driver_->GetData(RoboteqRobotDriverWrapper::kRearDriverName).IsError()); +} + +TEST_F(TestRoboteqRobotDriver, TurnOnEStop) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOnEStop()).Times(1); + EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOnEStop()).Times(1); + + EXPECT_NO_THROW(robot_driver_->TurnOnEStop()); +} + +TEST_F(TestRoboteqRobotDriver, TurnOffEStop) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOffEStop()).Times(1); + EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOffEStop()).Times(1); + + EXPECT_NO_THROW(robot_driver_->TurnOffEStop()); +} + +TEST_F(TestRoboteqRobotDriver, TurnOnEStopError) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOnEStop()) + .WillOnce(::testing::Throw(std::runtime_error(""))); + EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOnEStop()).Times(0); + + EXPECT_THROW(robot_driver_->TurnOnEStop(), std::runtime_error); +} + +TEST_F(TestRoboteqRobotDriver, TurnOffEStopError) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, TurnOffEStop()) + .WillOnce(::testing::Throw(std::runtime_error(""))); + EXPECT_CALL(*robot_driver_->mock_rear_driver, TurnOffEStop()).Times(0); + + EXPECT_THROW(robot_driver_->TurnOffEStop(), std::runtime_error); +} + +TEST_F(TestRoboteqRobotDriver, CommunicationError) +{ + EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(false)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(false)); + EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(false)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).WillOnce(::testing::Return(false)); + + ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState()); + EXPECT_FALSE(robot_driver_->CommunicationError()); + + EXPECT_CALL(*robot_driver_->mock_front_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsHeartbeatTimeout()) + .WillOnce(::testing::Return(true)); + EXPECT_CALL(*robot_driver_->mock_front_driver, IsCANError()).WillOnce(::testing::Return(true)); + EXPECT_CALL(*robot_driver_->mock_rear_driver, IsCANError()).WillOnce(::testing::Return(true)); + + ASSERT_NO_THROW(robot_driver_->UpdateCommunicationState()); + EXPECT_TRUE(robot_driver_->CommunicationError()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_lynx_system.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_lynx_system.cpp new file mode 100644 index 00000000..66da3df6 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_lynx_system.cpp @@ -0,0 +1,317 @@ +// 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 + +#include "husarion_ugv_hardware_interfaces/robot_system/lynx_system.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp" + +#include "utils/system_test_utils.hpp" +#include "utils/test_constants.hpp" + +class LynxSystemWrapper : public husarion_ugv_hardware_interfaces::LynxSystem +{ +public: + LynxSystemWrapper() : LynxSystem() + { + mock_robot_driver = + std::make_shared(); + mock_gpio_controller = + std::make_shared(); + mock_e_stop = std::make_shared(); + } + + void DefineRobotDriver() override { robot_driver_ = mock_robot_driver; } + void ConfigureGPIOController() override { gpio_controller_ = mock_gpio_controller; } + void ConfigureEStop() override { e_stop_ = mock_e_stop; } + + void ReadCANopenSettingsDriverCANIDs() { LynxSystem::ReadCANopenSettingsDriverCANIDs(); } + void UpdateHwStates() { LynxSystem::UpdateHwStates(); } + void UpdateMotorsStateDataTimedOut() { LynxSystem::UpdateMotorsStateDataTimedOut(); } + void UpdateDriverStateDataTimedOut() { LynxSystem::UpdateDriverStateDataTimedOut(); } + + void UpdateDriverStateMsg() { LynxSystem::UpdateDriverStateMsg(); } + void UpdateFlagErrors() { LynxSystem::UpdateFlagErrors(); } + std::vector GetSpeedCommands() const { return LynxSystem::GetSpeedCommands(); } + + void SetHwCommandsVelocities(std::vector & velocities) + { + hw_commands_velocities_[0] = velocities[0]; + hw_commands_velocities_[1] = velocities[1]; + hw_commands_velocities_[2] = velocities[2]; + hw_commands_velocities_[3] = velocities[3]; + } + + husarion_ugv_hardware_interfaces::CANopenSettings GetCANopenSettings() + { + return canopen_settings_; + } + std::vector GetHwStatesPositions() { return hw_states_positions_; } + std::vector GetHwStatesVelocities() { return hw_states_velocities_; } + std::vector GetHwStatesEfforts() { return hw_states_efforts_; } + + std::shared_ptr GetRoboteqErrorFilter() + { + return roboteq_error_filter_; + } + + std::shared_ptr + mock_robot_driver; + std::shared_ptr + mock_gpio_controller; + std::shared_ptr mock_e_stop; +}; + +class TestLynxSystem : public ::testing::Test +{ +public: + TestLynxSystem() + { + lynx_system_ = std::make_shared(); + + hardware_info_ = husarion_ugv_hardware_interfaces_test::GenerateDefaultHardwareInfo(); + hardware_info_.hardware_parameters.emplace("driver_can_id", "1"); + + lynx_system_->on_init(hardware_info_); + lynx_system_->on_configure(rclcpp_lifecycle::State()); + } + + ~TestLynxSystem() {} + +protected: + std::shared_ptr lynx_system_; + hardware_interface::HardwareInfo hardware_info_; +}; + +TEST_F(TestLynxSystem, ReadCANopenSettingsDriverCANIDs) +{ + ASSERT_NO_THROW(lynx_system_->ReadCANopenSettingsDriverCANIDs()); + + const auto canopen_settings = lynx_system_->GetCANopenSettings(); + + EXPECT_EQ(canopen_settings.driver_can_ids.size(), 1); + EXPECT_EQ( + canopen_settings.driver_can_ids.at(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT), 1); +} + +TEST_F(TestLynxSystem, UpdateHwStates) +{ + const std::int32_t left_pos = 10; + const std::int16_t left_vel = 20; + const std::int16_t left_eff = 30; + const std::int32_t right_pos = 40; + const std::int16_t right_vel = 50; + const std::int16_t right_eff = 60; + + const auto left_expected_pos = left_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad; + const auto left_expected_vel = left_vel * + husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto left_expected_eff = + left_eff * husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + const auto right_expected_pos = right_pos * + husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad; + const auto right_expected_vel = right_vel * + husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto right_expected_eff = + right_eff * husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + + husarion_ugv_hardware_interfaces::MotorDriverState left_motor_driver_state = { + left_pos, left_vel, left_eff, {0, 0}, {0, 0}}; + husarion_ugv_hardware_interfaces::MotorDriverState right_motor_driver_state = { + right_pos, right_vel, right_eff, {0, 0}, {0, 0}}; + + husarion_ugv_hardware_interfaces::DriverData roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + // left - channel 2, right - channel 1 + roboteq_data.SetMotorsStates(right_motor_driver_state, left_motor_driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + ASSERT_NO_THROW(lynx_system_->UpdateHwStates()); + + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[0], left_expected_pos); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[1], right_expected_pos); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[2], left_expected_pos); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesPositions()[3], right_expected_pos); + + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[0], left_expected_vel); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[1], right_expected_vel); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[2], left_expected_vel); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesVelocities()[3], right_expected_vel); + + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[0], left_expected_eff); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[1], right_expected_eff); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[2], left_expected_eff); + EXPECT_FLOAT_EQ(lynx_system_->GetHwStatesEfforts()[3], right_expected_eff); +} + +TEST_F(TestLynxSystem, UpdateMotorsStateDataTimedOut) +{ + husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state; + + husarion_ugv_hardware_interfaces::DriverData roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, true); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->UpdateMotorsStateDataTimedOut(); + + auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES)); + + EXPECT_TRUE(error); + + // check if reset error works + roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + lynx_system_->UpdateMotorsStateDataTimedOut(); + + error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES)); + + EXPECT_FALSE(error); +} + +TEST_F(TestLynxSystem, UpdateDriverStateMsg) +{ + // TODO requires subscribing to DriverState topic. Implement in the future or add abstraction for + // system_ros_interface_ +} + +TEST_F(TestLynxSystem, UpdateFlagErrors) +{ + auto driver_state = husarion_ugv_hardware_interfaces::DriverState(); + driver_state.fault_flags = 0b01; + + husarion_ugv_hardware_interfaces::DriverData roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->UpdateFlagErrors(); + + auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER)); + + EXPECT_TRUE(error); + + // check if reset error works + driver_state.fault_flags = 0; + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + lynx_system_->UpdateFlagErrors(); + + error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER)); + + EXPECT_FALSE(error); +} + +TEST_F(TestLynxSystem, UpdateDriverStateDataTimedOut) +{ + husarion_ugv_hardware_interfaces::DriverState driver_state; + + husarion_ugv_hardware_interfaces::DriverData roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetDriverState(driver_state, true); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->UpdateDriverStateDataTimedOut(); + + auto error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE)); + + EXPECT_TRUE(error); + + // check if reset error works + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *lynx_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::DEFAULT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + lynx_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + lynx_system_->UpdateDriverStateDataTimedOut(); + + error_map = lynx_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE)); + + EXPECT_FALSE(error); +} + +TEST_F(TestLynxSystem, GetSpeedCommands) +{ + const auto fl_v = 0.1; + const auto fr_v = 0.2; + const auto rl_v = 0.3; + const auto rr_v = 0.4; + + std::vector velocities = {fl_v, fr_v, rl_v, rr_v}; + lynx_system_->SetHwCommandsVelocities(velocities); + const auto speed_cmd = lynx_system_->GetSpeedCommands(); + + // only front left and front right motors are used for speed commands + ASSERT_EQ(speed_cmd.size(), 2); + EXPECT_FLOAT_EQ(speed_cmd[0], fl_v); + EXPECT_FLOAT_EQ(speed_cmd[1], fr_v); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + const auto result = RUN_ALL_TESTS(); + return result; +} diff --git a/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_panther_system.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_panther_system.cpp new file mode 100644 index 00000000..057b4b5f --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_panther_system.cpp @@ -0,0 +1,388 @@ +// 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 + +#include "husarion_ugv_hardware_interfaces/robot_system/panther_system.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/roboteq_robot_driver.hpp" + +#include "utils/system_test_utils.hpp" +#include "utils/test_constants.hpp" + +class PantherSystemWrapper : public husarion_ugv_hardware_interfaces::PantherSystem +{ +public: + PantherSystemWrapper() : PantherSystem() + { + mock_robot_driver = + std::make_shared(); + mock_gpio_controller = + std::make_shared(); + mock_e_stop = std::make_shared(); + + ON_CALL( + *mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT))) + .WillByDefault(::testing::ReturnRef(default_driver_data)); + ON_CALL( + *mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR))) + .WillByDefault(::testing::ReturnRef(default_driver_data)); + } + + void DefineRobotDriver() override { robot_driver_ = mock_robot_driver; } + void ConfigureGPIOController() override { gpio_controller_ = mock_gpio_controller; } + void ConfigureEStop() override { e_stop_ = mock_e_stop; } + + void ReadCANopenSettingsDriverCANIDs() { PantherSystem::ReadCANopenSettingsDriverCANIDs(); } + void UpdateHwStates() { PantherSystem::UpdateHwStates(); } + void UpdateMotorsStateDataTimedOut() { PantherSystem::UpdateMotorsStateDataTimedOut(); } + void UpdateDriverStateDataTimedOut() { PantherSystem::UpdateDriverStateDataTimedOut(); } + + void UpdateDriverStateMsg() { PantherSystem::UpdateDriverStateMsg(); } + void UpdateFlagErrors() { PantherSystem::UpdateFlagErrors(); } + std::vector GetSpeedCommands() const { return PantherSystem::GetSpeedCommands(); } + + void SetHwCommandsVelocities(std::vector & velocities) + { + hw_commands_velocities_[0] = velocities[0]; + hw_commands_velocities_[1] = velocities[1]; + hw_commands_velocities_[2] = velocities[2]; + hw_commands_velocities_[3] = velocities[3]; + } + + husarion_ugv_hardware_interfaces::CANopenSettings GetCANopenSettings() + { + return canopen_settings_; + } + std::vector GetHwStatesPositions() { return hw_states_positions_; } + std::vector GetHwStatesVelocities() { return hw_states_velocities_; } + std::vector GetHwStatesEfforts() { return hw_states_efforts_; } + + std::shared_ptr GetRoboteqErrorFilter() + { + return roboteq_error_filter_; + } + + std::shared_ptr + mock_robot_driver; + std::shared_ptr + mock_gpio_controller; + std::shared_ptr mock_e_stop; + +private: + husarion_ugv_hardware_interfaces::DriverData default_driver_data = + husarion_ugv_hardware_interfaces::DriverData( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); +}; + +class TestPantherSystem : public ::testing::Test +{ +public: + TestPantherSystem() + { + panther_system_ = std::make_shared(); + + hardware_info_ = husarion_ugv_hardware_interfaces_test::GenerateDefaultHardwareInfo(); + hardware_info_.hardware_parameters.emplace("front_driver_can_id", "1"); + hardware_info_.hardware_parameters.emplace("rear_driver_can_id", "2"); + + panther_system_->on_init(hardware_info_); + panther_system_->on_configure(rclcpp_lifecycle::State()); + } + + ~TestPantherSystem() {} + +protected: + std::shared_ptr panther_system_; + hardware_interface::HardwareInfo hardware_info_; +}; + +TEST_F(TestPantherSystem, ReadCANopenSettingsDriverCANIDs) +{ + ASSERT_NO_THROW(panther_system_->ReadCANopenSettingsDriverCANIDs()); + + const auto canopen_settings = panther_system_->GetCANopenSettings(); + + EXPECT_EQ(canopen_settings.driver_can_ids.size(), 2); + EXPECT_EQ( + canopen_settings.driver_can_ids.at(husarion_ugv_hardware_interfaces::DriverNames::FRONT), 1); + EXPECT_EQ( + canopen_settings.driver_can_ids.at(husarion_ugv_hardware_interfaces::DriverNames::REAR), 2); +} + +TEST_F(TestPantherSystem, UpdateHwStates) +{ + const std::int32_t fl_pos = 10; + const std::int16_t fl_vel = 20; + const std::int16_t fl_eff = 30; + const std::int32_t fr_pos = 40; + const std::int16_t fr_vel = 50; + const std::int16_t fr_eff = 60; + const std::int32_t rl_pos = 70; + const std::int16_t rl_vel = 80; + const std::int16_t rl_eff = 90; + const std::int32_t rr_pos = 100; + const std::int16_t rr_vel = 110; + const std::int16_t rr_eff = 120; + + const auto fl_expected_pos = fl_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad; + const auto fl_expected_vel = fl_vel * + husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto fl_expected_eff = fl_eff * + husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + const auto fr_expected_pos = fr_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad; + const auto fr_expected_vel = fr_vel * + husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto fr_expected_eff = fr_eff * + husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + const auto rl_expected_pos = rl_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad; + const auto rl_expected_vel = rl_vel * + husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto rl_expected_eff = rl_eff * + husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + const auto rr_expected_pos = rr_pos * husarion_ugv_hardware_interfaces_test::kRbtqPosFbToRad; + const auto rr_expected_vel = rr_vel * + husarion_ugv_hardware_interfaces_test::kRbtqVelFbToRadPerSec; + const auto rr_expected_eff = rr_eff * + husarion_ugv_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; + + husarion_ugv_hardware_interfaces::MotorDriverState fl_motor_driver_state = { + fl_pos, fl_vel, fl_eff, {0, 0}, {0, 0}}; + husarion_ugv_hardware_interfaces::MotorDriverState fr_motor_driver_state = { + fr_pos, fr_vel, fr_eff, {0, 0}, {0, 0}}; + husarion_ugv_hardware_interfaces::MotorDriverState rl_motor_driver_state = { + rl_pos, rl_vel, rl_eff, {0, 0}, {0, 0}}; + husarion_ugv_hardware_interfaces::MotorDriverState rr_motor_driver_state = { + rr_pos, rr_vel, rr_eff, {0, 0}, {0, 0}}; + + husarion_ugv_hardware_interfaces::DriverData front_roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + husarion_ugv_hardware_interfaces::DriverData rear_roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + // left - channel 2, right - channel 1 + front_roboteq_data.SetMotorsStates(fr_motor_driver_state, fl_motor_driver_state, false); + rear_roboteq_data.SetMotorsStates(rr_motor_driver_state, rl_motor_driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT))) + .WillOnce(::testing::ReturnRef(front_roboteq_data)); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR))) + .WillOnce(::testing::ReturnRef(rear_roboteq_data)); + + ASSERT_NO_THROW(panther_system_->UpdateHwStates()); + + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[0], fl_expected_pos); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[1], fr_expected_pos); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[2], rl_expected_pos); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesPositions()[3], rr_expected_pos); + + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[0], fl_expected_vel); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[1], fr_expected_vel); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[2], rl_expected_vel); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesVelocities()[3], rr_expected_vel); + + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[0], fl_expected_eff); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[1], fr_expected_eff); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[2], rl_expected_eff); + EXPECT_FLOAT_EQ(panther_system_->GetHwStatesEfforts()[3], rr_expected_eff); +} + +TEST_F(TestPantherSystem, UpdateMotorsStateDataTimedOut) +{ + husarion_ugv_hardware_interfaces::MotorDriverState motor_driver_state; + + husarion_ugv_hardware_interfaces::DriverData roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, true); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR))) + .Times(0); + + panther_system_->UpdateMotorsStateDataTimedOut(); + + auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES)); + + EXPECT_TRUE(error); + + // check if reset error works + roboteq_data.SetMotorsStates(motor_driver_state, motor_driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT))) + .Times(1); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR))) + .Times(1); + + panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + panther_system_->UpdateMotorsStateDataTimedOut(); + + error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_MOTOR_STATES)); + + EXPECT_FALSE(error); +} + +TEST_F(TestPantherSystem, UpdateDriverStateMsg) +{ + // TODO requires subscribing to DriverState topic. Implement in the future or add abstraction for + // system_ros_interface_ +} + +TEST_F(TestPantherSystem, UpdateFlagErrors) +{ + husarion_ugv_hardware_interfaces::DriverState driver_state; + driver_state.fault_flags = 0b01; + driver_state.script_flags = 0; + driver_state.runtime_stat_flag_channel_1 = 0; + driver_state.runtime_stat_flag_channel_2 = 0; + + husarion_ugv_hardware_interfaces::DriverData roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT))) + .Times(1); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + panther_system_->UpdateFlagErrors(); + + auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER)); + + EXPECT_TRUE(error); + + // check if reset error works + driver_state.fault_flags = 0; + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT))) + .Times(1); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + + panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + panther_system_->UpdateFlagErrors(); + + error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::ROBOTEQ_DRIVER)); + + EXPECT_FALSE(error); +} + +TEST_F(TestPantherSystem, UpdateDriverStateDataTimedOut) +{ + husarion_ugv_hardware_interfaces::DriverState driver_state; + + husarion_ugv_hardware_interfaces::DriverData roboteq_data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + roboteq_data.SetDriverState(driver_state, true); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR))) + .Times(0); + + panther_system_->UpdateDriverStateDataTimedOut(); + + auto error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + auto error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE)); + + EXPECT_TRUE(error); + + // check if reset error works + roboteq_data.SetDriverState(driver_state, false); + + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::FRONT))) + .WillOnce(::testing::ReturnRef(roboteq_data)); + EXPECT_CALL( + *panther_system_->mock_robot_driver, + GetData(::testing::Eq(husarion_ugv_hardware_interfaces::DriverNames::REAR))) + .Times(1); + + panther_system_->GetRoboteqErrorFilter()->SetClearErrorsFlag(); + panther_system_->UpdateDriverStateDataTimedOut(); + + error_map = panther_system_->GetRoboteqErrorFilter()->GetErrorMap(); + error = error_map.at(husarion_ugv_hardware_interfaces::error_filter_id_names.at( + husarion_ugv_hardware_interfaces::ErrorsFilterIds::READ_PDO_DRIVER_STATE)); + + EXPECT_FALSE(error); +} + +TEST_F(TestPantherSystem, GetSpeedCommands) +{ + const auto fl_v = 0.1; + const auto fr_v = 0.2; + const auto rl_v = 0.3; + const auto rr_v = 0.4; + + std::vector velocities = {fl_v, fr_v, rl_v, rr_v}; + panther_system_->SetHwCommandsVelocities(velocities); + const auto speed_cmd = panther_system_->GetSpeedCommands(); + + ASSERT_EQ(speed_cmd.size(), 4); + EXPECT_FLOAT_EQ(speed_cmd[0], fl_v); + EXPECT_FLOAT_EQ(speed_cmd[1], fr_v); + EXPECT_FLOAT_EQ(speed_cmd[2], rl_v); + EXPECT_FLOAT_EQ(speed_cmd[3], rr_v); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + const auto result = RUN_ALL_TESTS(); + return result; +} diff --git a/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_system_ros_interface.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_system_ros_interface.cpp new file mode 100644 index 00000000..6e3d15a1 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_system_ros_interface.cpp @@ -0,0 +1,208 @@ +// 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 + +#include + +#include "utils/test_constants.hpp" + +using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; + +class SystemROSInterfaceWrapper : public husarion_ugv_hardware_interfaces::SystemROSInterface +{ +public: + SystemROSInterfaceWrapper(const std::string & node_name) : SystemROSInterface(node_name) {} + + RobotDriverStateMsg & GetRobotDriverStateMsg() { return realtime_driver_state_publisher_->msg_; } +}; + +class TestSystemROSInterface : public ::testing::Test +{ +public: + TestSystemROSInterface() + { + system_ros_interface_ = std::make_unique("hardware_controller"); + } + + ~TestSystemROSInterface() { system_ros_interface_.reset(); } + +protected: + std::unique_ptr system_ros_interface_; +}; + +TEST(TestSystemROSInterfaceInitialization, NodeCreation) +{ + using husarion_ugv_hardware_interfaces::SystemROSInterface; + + std::vector node_names; + const std::string system_node_name = "hardware_controller"; + const std::string system_node_name_with_ns = "/" + system_node_name; + + rclcpp::Node::SharedPtr test_node = std::make_shared("test_system_node"); + + std::unique_ptr system_ros_interface; + + system_ros_interface = std::make_unique(system_node_name); + + node_names = test_node->get_node_names(); + ASSERT_TRUE( + std::find(node_names.begin(), node_names.end(), system_node_name_with_ns) != node_names.end()); + + system_ros_interface.reset(); + node_names = test_node->get_node_names(); + ASSERT_FALSE( + std::find(node_names.begin(), node_names.end(), system_node_name_with_ns) != node_names.end()); + + // Check if it is possible to create a node once again (if everything was cleaned up properly) + system_ros_interface = std::make_unique(system_node_name); + node_names = test_node->get_node_names(); + ASSERT_TRUE( + std::find(node_names.begin(), node_names.end(), system_node_name_with_ns) != node_names.end()); +} + +TEST_F(TestSystemROSInterface, UpdateMsgErrorFlags) +{ + husarion_ugv_hardware_interfaces::DriverData data( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings); + + husarion_ugv_hardware_interfaces::DriverState driver_state; + driver_state.fault_flags = 0b00000001; + driver_state.script_flags = 0b00000010; + driver_state.runtime_stat_flag_channel_1 = 0b00001000; + driver_state.runtime_stat_flag_channel_2 = 0b00000100; + + data.SetDriverState(driver_state, false); + + system_ros_interface_->UpdateMsgErrorFlags("driver", data); + + const auto driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg(); + + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.fault_flag.overheat); + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.script_flag.encoder_disconnected); + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.channel_2_motor_runtime_error.loop_error); + EXPECT_TRUE( + driver_state_msg.driver_states.at(0).state.channel_1_motor_runtime_error.safety_stop_active); +} + +TEST_F(TestSystemROSInterface, UpdateMsgDriversStates) +{ + husarion_ugv_hardware_interfaces::RoboteqDriverState state; + + const std::int16_t temp = 36; + const std::int16_t heatsink_temp = 37; + const std::uint16_t volt = 405; + const std::int16_t battery_current_1 = 15; + const std::int16_t battery_current_2 = 12; + + state.SetTemperature(temp); + state.SetHeatsinkTemperature(heatsink_temp); + state.SetVoltage(volt); + state.SetBatteryCurrent1(battery_current_1); + state.SetBatteryCurrent2(battery_current_2); + + system_ros_interface_->UpdateMsgDriversStates("driver", state); + + const auto driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg(); + + EXPECT_FLOAT_EQ( + static_cast(driver_state_msg.driver_states.at(0).state.temperature), temp); + EXPECT_FLOAT_EQ( + static_cast(driver_state_msg.driver_states.at(0).state.heatsink_temperature), + heatsink_temp); + EXPECT_FLOAT_EQ( + static_cast(driver_state_msg.driver_states.at(0).state.voltage * 10.0), volt); + EXPECT_FLOAT_EQ( + static_cast(driver_state_msg.driver_states.at(0).state.current * 10.0), + (battery_current_1 + battery_current_2)); +} + +TEST_F(TestSystemROSInterface, UpdateMsgErrors) +{ + husarion_ugv_hardware_interfaces::CANErrors can_errors; + can_errors.error = true; + + can_errors.write_pdo_cmds_error = true; + can_errors.read_pdo_motor_states_error = false; + can_errors.read_pdo_driver_state_error = false; + + husarion_ugv_hardware_interfaces::DriverCANErrors driver_can_errors; + driver_can_errors.motor_states_data_timed_out = true; + driver_can_errors.driver_state_data_timed_out = false; + driver_can_errors.can_error = false; + driver_can_errors.heartbeat_timeout = true; + + can_errors.driver_errors.emplace("driver", driver_can_errors); + + system_ros_interface_->UpdateMsgErrors(can_errors); + + const auto driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg(); + + EXPECT_TRUE(driver_state_msg.error); + + EXPECT_TRUE(driver_state_msg.write_pdo_cmds_error); + EXPECT_FALSE(driver_state_msg.read_pdo_motor_states_error); + EXPECT_FALSE(driver_state_msg.read_pdo_driver_state_error); + + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.motor_states_data_timed_out); + EXPECT_FALSE(driver_state_msg.driver_states.at(0).state.driver_state_data_timed_out); + EXPECT_FALSE(driver_state_msg.driver_states.at(0).state.can_error); + EXPECT_TRUE(driver_state_msg.driver_states.at(0).state.heartbeat_timeout); +} + +TEST_F(TestSystemROSInterface, CreateDriverStateEntryInMsg) +{ + const auto driver_1_name = "driver_1"; + const auto driver_2_name = "driver_2"; + const auto driver_3_name = "driver_3"; + + auto & driver_state_msg = system_ros_interface_->GetRobotDriverStateMsg(); + + ASSERT_EQ(driver_state_msg.driver_states.size(), 0); + + // check 3 different methods that can create a new entry in the message + system_ros_interface_->UpdateMsgErrorFlags( + driver_1_name, husarion_ugv_hardware_interfaces::DriverData( + husarion_ugv_hardware_interfaces_test::kDrivetrainSettings)); + system_ros_interface_->UpdateMsgDriversStates(driver_2_name, {}); + + husarion_ugv_hardware_interfaces::CANErrors can_errors; + can_errors.driver_errors.emplace( + driver_3_name, husarion_ugv_hardware_interfaces::DriverCANErrors()); + system_ros_interface_->UpdateMsgErrors(can_errors); + + EXPECT_EQ(driver_state_msg.driver_states.size(), 3); + EXPECT_EQ(driver_state_msg.driver_states.at(0).name, driver_1_name); + EXPECT_EQ(driver_state_msg.driver_states.at(1).name, driver_2_name); + EXPECT_EQ(driver_state_msg.driver_states.at(2).name, driver_3_name); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + + auto run_tests = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return run_tests; +} diff --git a/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_ugv_system.cpp b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_ugv_system.cpp new file mode 100644 index 00000000..3170ae53 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/unit/robot_system/test_ugv_system.cpp @@ -0,0 +1,381 @@ +// 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 +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp" + +#include "utils/system_test_utils.hpp" + +class MockUGVSystem : public husarion_ugv_hardware_interfaces::UGVSystem +{ +public: + MockUGVSystem(const std::vector & joint_order) + : husarion_ugv_hardware_interfaces::UGVSystem(joint_order) + { + mock_robot_driver_ = + std::make_shared(); + mock_gpio_controller_ = + std::make_shared(); + mock_e_stop_ = std::make_shared(); + + ON_CALL(*this, DefineRobotDriver()).WillByDefault(::testing::Invoke([&]() { + robot_driver_ = mock_robot_driver_; + })); + ON_CALL(*this, ConfigureGPIOController()).WillByDefault(::testing::Invoke([&]() { + gpio_controller_ = mock_gpio_controller_; + })); + ON_CALL(*this, ConfigureEStop()).WillByDefault(::testing::Invoke([&]() { + e_stop_ = mock_e_stop_; + })); + } + + MOCK_METHOD(void, ReadCANopenSettingsDriverCANIDs, (), (override)); + MOCK_METHOD(void, ConfigureGPIOController, (), (override)); + MOCK_METHOD(void, ConfigureEStop, (), (override)); + + MOCK_METHOD(void, DefineRobotDriver, (), (override)); + + MOCK_METHOD(void, UpdateHwStates, (), (override)); + MOCK_METHOD(void, UpdateMotorsStateDataTimedOut, (), (override)); + MOCK_METHOD(void, UpdateDriverStateMsg, (), (override)); + MOCK_METHOD(void, UpdateFlagErrors, (), (override)); + MOCK_METHOD(void, UpdateDriverStateDataTimedOut, (), (override)); + + MOCK_METHOD(std::vector, GetSpeedCommands, (), (const, override)); + + MOCK_METHOD(void, DiagnoseErrors, (diagnostic_updater::DiagnosticStatusWrapper &), (override)); + MOCK_METHOD(void, DiagnoseStatus, (diagnostic_updater::DiagnosticStatusWrapper &), (override)); + + void DefaultDefineRobotDriver() + { + robot_driver_ = + std::make_shared(); + } + + void SetHwCommandVelocity(const std::vector & velocity) + { + hw_commands_velocities_ = velocity; + } + void SetHwStatePosition(const std::vector & position) { hw_states_positions_ = position; } + void SetHwStateVelocity(const std::vector & velocity) + { + hw_states_velocities_ = velocity; + } + void SetHwStateEffort(const std::vector & effort) { hw_states_efforts_ = effort; } + + std::shared_ptr + GetMockRobotDriver() + { + return mock_robot_driver_; + } + + std::shared_ptr + GetMockGPIOController() + { + return mock_gpio_controller_; + } + + std::shared_ptr GetMockEStop() + { + return mock_e_stop_; + } + + using NiceMock = testing::NiceMock; + +private: + std::shared_ptr + mock_robot_driver_; + std::shared_ptr + mock_gpio_controller_; + std::shared_ptr mock_e_stop_; +}; + +class TestUGVSystem : public ::testing::Test +{ +public: + TestUGVSystem() + { + ugv_system_ = + std::make_shared(std::vector{"fl", "fr", "rl", "rr"}); + + hardware_info_ = husarion_ugv_hardware_interfaces_test::GenerateDefaultHardwareInfo(); + } + + ~TestUGVSystem() {} + +protected: + std::shared_ptr ugv_system_; + hardware_interface::HardwareInfo hardware_info_; +}; + +TEST_F(TestUGVSystem, OnInit) +{ + EXPECT_CALL(*ugv_system_, ReadCANopenSettingsDriverCANIDs()).Times(1); + + auto callback_return = ugv_system_->on_init(hardware_info_); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, OnConfigure) +{ + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + EXPECT_CALL(*ugv_system_, DefineRobotDriver()).WillOnce(::testing::Invoke([&]() { + ugv_system_->DefaultDefineRobotDriver(); + })); + EXPECT_CALL(*ugv_system_, ConfigureGPIOController()).Times(1); + EXPECT_CALL(*ugv_system_, ConfigureEStop()).Times(1); + auto callback_return = ugv_system_->on_configure(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, OnCleanup) +{ + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1); + auto callback_return = ugv_system_->on_cleanup(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, OnActivate) +{ + rclcpp::init(0, nullptr); + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Activate()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockGPIOController(), QueryControlInterfaceIOStates()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1); + auto callback_return = ugv_system_->on_activate(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); + + rclcpp::shutdown(); +} + +TEST_F(TestUGVSystem, OnDeactivate) +{ + rclcpp::init(0, nullptr); + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockEStop(), TriggerEStop()).Times(1); + auto callback_return = ugv_system_->on_deactivate(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); + + rclcpp::shutdown(); +} + +TEST_F(TestUGVSystem, OnShutdown) +{ + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockEStop(), TriggerEStop()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1); + auto callback_return = ugv_system_->on_shutdown(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, OnError) +{ + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockEStop(), TriggerEStop()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1); + auto callback_return = ugv_system_->on_error(rclcpp_lifecycle::State()); + + EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS); +} + +TEST_F(TestUGVSystem, ExportStateInterfacesInitialValues) +{ + std::vector prefixes = {"fl", "fr", "rl", "rr"}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + std::vector state_interfaces = + ugv_system_->export_state_interfaces(); + + ASSERT_EQ(state_interfaces.size(), 12); + + int i = 0; + for (const auto & prefix : prefixes) { + EXPECT_EQ( + state_interfaces[i].get_name(), + prefix + "_wheel_joint/" + hardware_interface::HW_IF_POSITION); + EXPECT_EQ( + state_interfaces[i + 1].get_name(), + prefix + "_wheel_joint/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + state_interfaces[i + 2].get_name(), + prefix + "_wheel_joint/" + hardware_interface::HW_IF_EFFORT); + + i += 3; + } + + auto all_interfaces_are_nan = std::all_of( + state_interfaces.begin(), state_interfaces.end(), + [](const hardware_interface::StateInterface & state) { return std::isnan(state.get_value()); }); + + EXPECT_TRUE(all_interfaces_are_nan); +} + +TEST_F(TestUGVSystem, ExportStateInterfaces) +{ + std::vector position = {1.0, 2.0, 3.0, 4.0}; + std::vector velocity = {5.0, 6.0, 7.0, 8.0}; + std::vector effort = {9.0, 10.0, 11.0, 12.0}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + ugv_system_->SetHwStatePosition(position); + ugv_system_->SetHwStateVelocity(velocity); + ugv_system_->SetHwStateEffort(effort); + + std::vector state_interfaces = + ugv_system_->export_state_interfaces(); + + ASSERT_EQ(state_interfaces.size(), 12); + + for (std::size_t i = 0; i < 4; i++) { + EXPECT_FLOAT_EQ(state_interfaces[i * 3].get_value(), position[i]); + EXPECT_FLOAT_EQ(state_interfaces[i * 3 + 1].get_value(), velocity[i]); + EXPECT_FLOAT_EQ(state_interfaces[i * 3 + 2].get_value(), effort[i]); + } +} + +TEST_F(TestUGVSystem, ExportCommandInterfacesInitialValues) +{ + std::vector prefixes = {"fl", "fr", "rl", "rr"}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + std::vector command_interfaces = + ugv_system_->export_command_interfaces(); + + ASSERT_EQ(command_interfaces.size(), 4); + + EXPECT_EQ( + command_interfaces[0].get_name(), + std::string("fl_wheel_joint/") + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + command_interfaces[1].get_name(), + std::string("fr_wheel_joint/") + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + command_interfaces[2].get_name(), + std::string("rl_wheel_joint/") + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + command_interfaces[3].get_name(), + std::string("rr_wheel_joint/") + hardware_interface::HW_IF_VELOCITY); + + std::for_each( + command_interfaces.begin(), command_interfaces.end(), + [](const hardware_interface::CommandInterface & command) { + EXPECT_FLOAT_EQ(command.get_value(), 0.0); + }); +} + +TEST_F(TestUGVSystem, ExportCommandInterfaces) +{ + std::vector velocity = {1.0, 2.0, 3.0, 4.0}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + + ugv_system_->SetHwCommandVelocity(velocity); + + std::vector command_interfaces = + ugv_system_->export_command_interfaces(); + + ASSERT_EQ(command_interfaces.size(), 4); + + for (std::size_t i = 0; i < 4; i++) { + EXPECT_FLOAT_EQ(command_interfaces[i].get_value(), velocity[i]); + } +} + +TEST_F(TestUGVSystem, Read) +{ + rclcpp::init(0, nullptr); + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), UpdateMotorsState()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateHwStates()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateMotorsStateDataTimedOut()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), UpdateDriversState()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateDriverStateMsg()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateFlagErrors()).Times(1); + EXPECT_CALL(*ugv_system_, UpdateDriverStateDataTimedOut()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1); + + auto callback_return = ugv_system_->read( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration(0, 0)); + + EXPECT_EQ(callback_return, hardware_interface::return_type::OK); + + rclcpp::shutdown(); +} + +TEST_F(TestUGVSystem, Write) +{ + rclcpp::init(0, nullptr); + + const auto velocity = std::vector{1.0, 2.0, 3.0, 4.0}; + + ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_)); + ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State())); + ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State())); + + EXPECT_CALL(*ugv_system_, GetSpeedCommands()).WillOnce(::testing::Return(velocity)); + EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1); + EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), SendSpeedCommands(velocity)).Times(1); + + auto callback_return = ugv_system_->write( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration(0, 0)); + + EXPECT_EQ(callback_return, hardware_interface::return_type::OK); + + rclcpp::shutdown(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + const auto result = RUN_ALL_TESTS(); + return result; +} diff --git a/husarion_ugv_hardware_interfaces/test/utils/fake_can_socket.hpp b/husarion_ugv_hardware_interfaces/test/utils/fake_can_socket.hpp new file mode 100644 index 00000000..0cb07b86 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/utils/fake_can_socket.hpp @@ -0,0 +1,80 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_ + +#include +#include + +#include + +namespace husarion_ugv_hardware_interfaces_test +{ + +class FakeCANSocket +{ +public: + FakeCANSocket(const std::string & can_interface_name) + : can_interface_name_(can_interface_name), can_device_created_(false) + { + } + + ~FakeCANSocket() { Deinitialize(); } + + void Initialize() + { + if (system("sudo modprobe vcan") != 0) { + throw std::runtime_error("Failed to load vcan module"); + } + + // if link already exists, do not create it + const auto check_command = "ip link show " + can_interface_name_ + " > /dev/null 2>&1"; + if (std::system(check_command.c_str()) != 0) { + const auto command = "sudo ip link add dev " + can_interface_name_ + " type vcan"; + if (system(command.c_str()) != 0) { + throw std::runtime_error("Failed to add vcan device"); + } + } + + can_device_created_ = true; + + const auto command = "sudo ip link set up " + can_interface_name_; + if (system(command.c_str()) != 0) { + throw std::runtime_error("Failed to set up vcan device"); + } + } + + void Deinitialize() + { + if (!can_device_created_) { + return; + } + + std::string command = "sudo ip link delete " + can_interface_name_; + if (system(command.c_str()) != 0) { + throw std::runtime_error("Failed to delete vcan device"); + } + + can_device_created_ = false; + } + +private: + const std::string can_interface_name_; + bool can_device_created_; +}; + +} // namespace husarion_ugv_hardware_interfaces_test + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_FAKE_CAN_SOCKET_HPP_ diff --git a/husarion_ugv_hardware_interfaces/test/utils/mock_driver.hpp b/husarion_ugv_hardware_interfaces/test/utils/mock_driver.hpp new file mode 100644 index 00000000..10f11674 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/utils/mock_driver.hpp @@ -0,0 +1,83 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_ + +#include +#include +#include +#include + +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/driver.hpp" + +namespace husarion_ugv_hardware_interfaces_test +{ + +class MockDriver : public husarion_ugv_hardware_interfaces::DriverInterface +{ +public: + MockDriver() + { + ON_CALL(*this, Boot()).WillByDefault(::testing::Invoke([]() { + std::promise promise; + promise.set_value(); + return promise.get_future(); + })); + } + + MOCK_METHOD(std::future, Boot, (), (override)); + MOCK_METHOD(bool, IsCANError, (), (const, override)); + MOCK_METHOD(bool, IsHeartbeatTimeout, (), (const, override)); + + MOCK_METHOD(husarion_ugv_hardware_interfaces::DriverState, ReadState, (), (override)); + MOCK_METHOD(void, ResetScript, (), (override)); + MOCK_METHOD(void, TurnOnEStop, (), (override)); + MOCK_METHOD(void, TurnOffEStop, (), (override)); + + std::shared_ptr GetMotorDriver( + const std::string & name) override + { + return motor_drivers_.at(name); + } + + void AddMotorDriver( + const std::string name, + std::shared_ptr motor_driver) override + { + motor_drivers_.emplace(name, motor_driver); + } + + using NiceMock = testing::NiceMock; + +private: + std::map> + motor_drivers_; +}; + +class MockMotorDriver : public husarion_ugv_hardware_interfaces::MotorDriverInterface +{ +public: + MOCK_METHOD(husarion_ugv_hardware_interfaces::MotorDriverState, ReadState, (), (override)); + MOCK_METHOD(void, SendCmdVel, (const std::int32_t cmd), (override)); + MOCK_METHOD(void, TurnOnSafetyStop, (), (override)); + + using NiceMock = testing::NiceMock; +}; + +} // namespace husarion_ugv_hardware_interfaces_test + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_DRIVER_HPP_ diff --git a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp b/husarion_ugv_hardware_interfaces/test/utils/mock_roboteq.hpp similarity index 82% rename from panther_hardware_interfaces/test/utils/roboteqs_mock.hpp rename to husarion_ugv_hardware_interfaces/test/utils/mock_roboteq.hpp index da6b4bb2..763813f9 100644 --- a/panther_hardware_interfaces/test/utils/roboteqs_mock.hpp +++ b/husarion_ugv_hardware_interfaces/test/utils/mock_roboteq.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_ROBOTEQ_MOCK_HPP_ -#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_ROBOTEQ_MOCK_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_ #include #include @@ -23,15 +23,15 @@ #include -#include -#include -#include -#include -#include -#include -#include +#include "lely/coapp/slave.hpp" +#include "lely/ev/loop.hpp" +#include "lely/io2/linux/can.hpp" +#include "lely/io2/posix/poll.hpp" +#include "lely/io2/sys/io.hpp" +#include "lely/io2/sys/sigset.hpp" +#include "lely/io2/sys/timer.hpp" -namespace panther_hardware_interfaces_test +namespace husarion_ugv_hardware_interfaces_test { enum class DriverFaultFlags { @@ -258,16 +258,16 @@ class RoboteqSlave : public lely::canopen::BasicSlave }; /** - * @brief Class that simulates two Roboteqs + * @brief Class that simulates Roboteq controller */ -class RoboteqsMock +class MockRoboteq { public: - RoboteqsMock() {} - ~RoboteqsMock() {} + MockRoboteq() {} + ~MockRoboteq() {} /** - * @brief Starts CAN communication and creates two simulated Roboteqs, that publish PDOs with set + * @brief Starts CAN communication and creates a simulated Roboteq, that publish PDOs with set * frequencies * * @param motors_states_period period of motors states publishing thread @@ -284,44 +284,30 @@ class RoboteqsMock std::thread([this, motors_states_period, driver_state_period]() { std::string slave_eds_path = std::filesystem::path( - ament_index_cpp::get_package_share_directory("panther_hardware_interfaces")) / + ament_index_cpp::get_package_share_directory("husarion_ugv_hardware_interfaces")) / "config" / "roboteq_motor_controllers_v80_21a.eds"; std::string slave1_eds_bin_path = std::filesystem::path( - ament_index_cpp::get_package_share_directory("panther_hardware_interfaces")) / + ament_index_cpp::get_package_share_directory("husarion_ugv_hardware_interfaces")) / "test" / "config" / "slave_1.bin"; - std::string slave2_eds_bin_path = - std::filesystem::path( - ament_index_cpp::get_package_share_directory("panther_hardware_interfaces")) / - "test" / "config" / "slave_2.bin"; - lely::io::IoGuard io_guard; lely::io::Poll poll(*ctx_); lely::ev::Loop loop(poll.get_poll()); auto exec = loop.get_executor(); lely::io::Timer timer(poll, exec, CLOCK_MONOTONIC); - lely::io::CanController ctrl("panther_can"); + lely::io::CanController ctrl("robot_can"); lely::io::CanChannel chan1(poll, exec); chan1.open(ctrl); lely::io::Timer timer1(poll, exec, CLOCK_MONOTONIC); - front_driver_ = std::make_shared( + driver_ = std::make_shared( timer1, chan1, slave_eds_path, slave1_eds_bin_path, 1); - lely::io::CanChannel chan2(poll, exec); - chan2.open(ctrl); - lely::io::Timer timer2(poll, exec, CLOCK_MONOTONIC); - rear_driver_ = std::make_shared( - timer2, chan2, slave_eds_path, slave2_eds_bin_path, 2); - - front_driver_->Reset(); - rear_driver_->Reset(); - front_driver_->InitializeValues(); - rear_driver_->InitializeValues(); - front_driver_->StartPublishing(motors_states_period, driver_state_period); - rear_driver_->StartPublishing(motors_states_period, driver_state_period); + driver_->Reset(); + driver_->InitializeValues(); + driver_->StartPublishing(motors_states_period, driver_state_period); { std::lock_guard lck_g(canopen_communication_started_mtx_); @@ -331,8 +317,7 @@ class RoboteqsMock loop.run(); - front_driver_->StopPublishing(); - rear_driver_->StopPublishing(); + driver_->StopPublishing(); }); if (!canopen_communication_started_.load()) { @@ -346,7 +331,7 @@ class RoboteqsMock } /** - * @brief Stops CAN communication and removes simulated Roboteqs + * @brief Stops CAN communication and removes simulated Roboteq */ void Stop() { @@ -355,14 +340,12 @@ class RoboteqsMock canopen_communication_thread_.join(); } - front_driver_.reset(); - rear_driver_.reset(); + driver_.reset(); canopen_communication_started_.store(false); } - std::shared_ptr GetFrontDriver() { return front_driver_; } - std::shared_ptr GetRearDriver() { return rear_driver_; } + std::shared_ptr GetDriver() { return driver_; } private: std::shared_ptr ctx_; @@ -373,10 +356,9 @@ class RoboteqsMock std::condition_variable canopen_communication_started_cond_; std::mutex canopen_communication_started_mtx_; - std::shared_ptr front_driver_; - std::shared_ptr rear_driver_; + std::shared_ptr driver_; }; -} // namespace panther_hardware_interfaces_test +} // namespace husarion_ugv_hardware_interfaces_test -#endif // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_ROBOTEQ_MOCK_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_MOCK_ROBOTEQ_HPP_ diff --git a/husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp b/husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp new file mode 100644 index 00000000..1b93e608 --- /dev/null +++ b/husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp @@ -0,0 +1,178 @@ +// 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 HUSARION_UGV_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_ + +#include +#include +#include +#include + +#include + +#include "husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_controller.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/robot_driver/robot_driver.hpp" +#include "husarion_ugv_hardware_interfaces/robot_system/system_e_stop.hpp" + +#include "utils/mock_driver.hpp" + +namespace husarion_ugv_hardware_interfaces_test +{ + +class MockRobotDriver : public husarion_ugv_hardware_interfaces::RobotDriverInterface +{ +public: + MOCK_METHOD(void, Initialize, (), (override)); + MOCK_METHOD(void, Deinitialize, (), (override)); + MOCK_METHOD(void, Activate, (), (override)); + MOCK_METHOD(void, UpdateCommunicationState, (), (override)); + MOCK_METHOD(void, UpdateMotorsState, (), (override)); + MOCK_METHOD(void, UpdateDriversState, (), (override)); + MOCK_METHOD( + const husarion_ugv_hardware_interfaces::DriverData &, GetData, (const std::string &), + (override)); + MOCK_METHOD(void, SendSpeedCommands, (const std::vector &), (override)); + MOCK_METHOD(void, TurnOnEStop, (), (override)); + MOCK_METHOD(void, TurnOffEStop, (), (override)); + MOCK_METHOD(void, AttemptErrorFlagReset, (), (override)); + MOCK_METHOD(bool, CommunicationError, (), (override)); + + using NiceMock = testing::NiceMock; +}; + +class MockGPIODriver : public husarion_ugv_hardware_interfaces::GPIODriverInterface +{ +public: + MOCK_METHOD(void, GPIOMonitorEnable, (const bool, const unsigned), (override)); + MOCK_METHOD( + void, ConfigureEdgeEventCallback, + (const std::function &), (override)); + MOCK_METHOD( + void, ChangePinDirection, + (const husarion_ugv_hardware_interfaces::GPIOPin, const gpiod::line::direction), (override)); + MOCK_METHOD( + bool, IsPinAvailable, (const husarion_ugv_hardware_interfaces::GPIOPin), (const, override)); + MOCK_METHOD(bool, IsPinActive, (const husarion_ugv_hardware_interfaces::GPIOPin), (override)); + MOCK_METHOD( + bool, SetPinValue, + (const husarion_ugv_hardware_interfaces::GPIOPin, const bool, + const std::chrono::milliseconds & pin_validation_wait_time), + (override)); + + using NiceMock = testing::NiceMock; +}; + +class MockGPIOController : public husarion_ugv_hardware_interfaces::GPIOControllerInterface +{ +public: + MockGPIOController() : GPIOControllerInterface() + { + gpio_driver_ = std::make_shared<::testing::NiceMock>(); + } + + MOCK_METHOD(void, Start, (), (override)); + MOCK_METHOD(void, EStopTrigger, (), (override)); + MOCK_METHOD(void, EStopReset, (), (override)); + MOCK_METHOD(bool, MotorPowerEnable, (const bool), (override)); + MOCK_METHOD(bool, FanEnable, (const bool), (override)); + MOCK_METHOD(bool, AUXPowerEnable, (const bool), (override)); + MOCK_METHOD(bool, DigitalPowerEnable, (const bool), (override)); + MOCK_METHOD(bool, ChargerEnable, (const bool), (override)); + MOCK_METHOD(bool, LEDControlEnable, (const bool), (override)); + MOCK_METHOD( + (std::unordered_map), + QueryControlInterfaceIOStates, (), (const, override)); + + using NiceMock = testing::NiceMock; +}; + +class MockEStop : public husarion_ugv_hardware_interfaces::EStopInterface +{ +public: + MockEStop() : EStopInterface() {} + + MOCK_METHOD(bool, ReadEStopState, (), (override)); + MOCK_METHOD(void, TriggerEStop, (), (override)); + MOCK_METHOD(void, ResetEStop, (), (override)); + + using NiceMock = testing::NiceMock; +}; + +hardware_interface::HardwareInfo GenerateDefaultHardwareInfo() +{ + hardware_interface::HardwareInfo hardware_info; + hardware_info.name = "test"; + hardware_info.hardware_class_type = "UGVSystem"; + + hardware_interface::InterfaceInfo vel_command_interface; + vel_command_interface.name = "velocity"; + hardware_interface::InterfaceInfo pos_state_interface; + pos_state_interface.name = "position"; + hardware_interface::InterfaceInfo vel_state_interface; + vel_state_interface.name = "velocity"; + hardware_interface::InterfaceInfo eff_state_interface; + eff_state_interface.name = "effort"; + + hardware_interface::ComponentInfo wheel_joint; + wheel_joint.command_interfaces = {vel_command_interface}; + wheel_joint.state_interfaces = {pos_state_interface, vel_state_interface, eff_state_interface}; + + auto fl_wheel_joint = wheel_joint; + fl_wheel_joint.name = "fl_wheel_joint"; + auto fr_wheel_joint = wheel_joint; + fr_wheel_joint.name = "fr_wheel_joint"; + auto rl_wheel_joint = wheel_joint; + rl_wheel_joint.name = "rl_wheel_joint"; + auto rr_wheel_joint = wheel_joint; + rr_wheel_joint.name = "rr_wheel_joint"; + + hardware_info.joints = {fl_wheel_joint, fr_wheel_joint, rl_wheel_joint, rr_wheel_joint}; + + std::unordered_map hardware_paremeters = { + // drivetrain settings + {"motor_torque_constant", "0.11"}, + {"gear_ratio", "30.08"}, + {"gearbox_efficiency", "0.75"}, + {"encoder_resolution", "1600"}, + {"max_rpm_motor_speed", "3600.0"}, + + // CANopen settings + {"can_interface_name", "robot_can"}, + {"master_can_id", "3"}, + {"pdo_motor_states_timeout_ms", "15"}, + {"pdo_driver_state_timeout_ms", "75"}, + {"sdo_operation_timeout_ms", "100"}, + + // Driver states update frequency + {"driver_states_update_frequency", "20.0"}, + + // Roboteq initialization and activation attempts + {"max_roboteq_initialization_attempts", "1"}, + {"max_roboteq_activation_attempts", "1"}, + + // Roboteq error filter params + {"max_write_pdo_cmds_errors_count", "1"}, + {"max_read_pdo_motor_states_errors_count", "1"}, + {"max_read_pdo_driver_state_errors_count", "1"}, + }; + + hardware_info.hardware_parameters = hardware_paremeters; + + return hardware_info; +} + +} // namespace husarion_ugv_hardware_interfaces_test + +#endif // HUSARION_UGV_HARDWARE_INTERFACES_TEST_SYSTEM_TEST_UTILS_HPP_ diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/husarion_ugv_hardware_interfaces/test/utils/test_constants.hpp similarity index 70% rename from panther_hardware_interfaces/test/utils/test_constants.hpp rename to husarion_ugv_hardware_interfaces/test/utils/test_constants.hpp index 92e4fe83..af66e9c3 100644 --- a/panther_hardware_interfaces/test/utils/test_constants.hpp +++ b/husarion_ugv_hardware_interfaces/test/utils/test_constants.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_ -#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_ +#ifndef HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_ +#define HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_ #include #include @@ -21,28 +21,27 @@ #include #include -#include -#include +#include +#include -namespace panther_hardware_interfaces_test +namespace husarion_ugv_hardware_interfaces_test { -const panther_hardware_interfaces::CANopenSettings kCANopenSettings{ - "panther_can", +const husarion_ugv_hardware_interfaces::CANopenSettings kCANopenSettings{ + "robot_can", 3, - 1, - 2, + {{"default", 1}, {"front", 1}, {"rear", 2}}, std::chrono::milliseconds(15), std::chrono::milliseconds(75), std::chrono::milliseconds(100), }; -const panther_hardware_interfaces::DrivetrainSettings kDrivetrainSettings{ +const husarion_ugv_hardware_interfaces::DrivetrainSettings kDrivetrainSettings{ 0.11, 30.08, 0.75, 1600.0, 3600.0}; constexpr float kRadPerSecToRbtqCmd = 30.08 * (1.0 / (2.0 * M_PI)) * 60.0 * (1000.0 / 3600.0); constexpr float kRbtqPosFbToRad = (1. / 1600) * (1.0 / 30.08) * (2.0 * M_PI); -constexpr float kRbtqVelFbToRadPerSec = (1. / 30.08) * (1. / 60.) * (2.0 * M_PI); +constexpr float kRbtqVelFbToRadPerSec = (3600. / 1000.) * (1. / 30.08) * (1. / 60.) * (2.0 * M_PI); constexpr float kRbtqCurrentFbToNewtonMeters = (1. / 10.) * 0.11 * 30.08 * 0.75; const std::string kPantherSystemName = "wheels"; @@ -64,17 +63,16 @@ const std::string kJointInterfaces = )"; const std::string kPluginName = - R"(panther_hardware_interfaces/PantherSystem + R"(husarion_ugv_hardware_interfaces/PantherSystem )"; const std::map kDefaultParamMap = { - {"panther_version", "1.2"}, {"encoder_resolution", "1600"}, {"gear_ratio", "30.08"}, {"gearbox_efficiency", "0.75"}, {"motor_torque_constant", "0.11"}, {"max_rpm_motor_speed", "3600.0"}, - {"can_interface_name", "panther_can"}, + {"can_interface_name", "robot_can"}, {"master_can_id", "3"}, {"front_driver_can_id", "1"}, {"rear_driver_can_id", "2"}, @@ -92,8 +90,8 @@ const std::map kDefaultParamMap = { const std::vector kDefaultJoints = { "fl_wheel_joint", "fr_wheel_joint", "rl_wheel_joint", "rr_wheel_joint"}; -const std::string kMotorControllersStateTopic = "/hardware_controller/motor_controllers_state"; +const std::string kRobotDriverStateTopic = "/hardware_controller/robot_driver_state"; -} // namespace panther_hardware_interfaces_test +} // namespace husarion_ugv_hardware_interfaces_test -#endif // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_ +#endif // HUSARION_UGV_HARDWARE_INTERFACES_TEST_UTILS_TEST_CONSTANTS_HPP_ diff --git a/panther_lights/.docs/AUTONOMOUS_ACTION.webp b/husarion_ugv_lights/.docs/AUTONOMOUS_ACTION.webp similarity index 100% rename from panther_lights/.docs/AUTONOMOUS_ACTION.webp rename to husarion_ugv_lights/.docs/AUTONOMOUS_ACTION.webp diff --git a/panther_lights/.docs/BATTERY_STATE.webp b/husarion_ugv_lights/.docs/BATTERY_STATE.webp similarity index 100% rename from panther_lights/.docs/BATTERY_STATE.webp rename to husarion_ugv_lights/.docs/BATTERY_STATE.webp diff --git a/husarion_ugv_lights/.docs/CHARGING_BATTERY.webp b/husarion_ugv_lights/.docs/CHARGING_BATTERY.webp new file mode 100644 index 00000000..2711b8fb Binary files /dev/null and b/husarion_ugv_lights/.docs/CHARGING_BATTERY.webp differ diff --git a/panther_lights/.docs/CRITICAL_BATTERY.webp b/husarion_ugv_lights/.docs/CRITICAL_BATTERY.webp similarity index 100% rename from panther_lights/.docs/CRITICAL_BATTERY.webp rename to husarion_ugv_lights/.docs/CRITICAL_BATTERY.webp diff --git a/husarion_ugv_lights/.docs/ERROR.webp b/husarion_ugv_lights/.docs/ERROR.webp new file mode 100644 index 00000000..cfc5f131 Binary files /dev/null and b/husarion_ugv_lights/.docs/ERROR.webp differ diff --git a/panther_lights/.docs/E_STOP.webp b/husarion_ugv_lights/.docs/E_STOP.webp similarity index 100% rename from panther_lights/.docs/E_STOP.webp rename to husarion_ugv_lights/.docs/E_STOP.webp diff --git a/husarion_ugv_lights/.docs/GOAL_ACHIEVED.webp b/husarion_ugv_lights/.docs/GOAL_ACHIEVED.webp new file mode 100644 index 00000000..2f862a57 Binary files /dev/null and b/husarion_ugv_lights/.docs/GOAL_ACHIEVED.webp differ diff --git a/panther_lights/.docs/LOW_BATTERY.webp b/husarion_ugv_lights/.docs/LOW_BATTERY.webp similarity index 100% rename from panther_lights/.docs/LOW_BATTERY.webp rename to husarion_ugv_lights/.docs/LOW_BATTERY.webp diff --git a/panther_lights/.docs/MANUAL_ACTION.webp b/husarion_ugv_lights/.docs/MANUAL_ACTION.webp similarity index 100% rename from panther_lights/.docs/MANUAL_ACTION.webp rename to husarion_ugv_lights/.docs/MANUAL_ACTION.webp diff --git a/panther_lights/.docs/READY.webp b/husarion_ugv_lights/.docs/READY.webp similarity index 100% rename from panther_lights/.docs/READY.webp rename to husarion_ugv_lights/.docs/READY.webp diff --git a/panther_lights/CHANGELOG.rst b/husarion_ugv_lights/CHANGELOG.rst similarity index 100% rename from panther_lights/CHANGELOG.rst rename to husarion_ugv_lights/CHANGELOG.rst diff --git a/panther_lights/CMakeLists.txt b/husarion_ugv_lights/CMakeLists.txt similarity index 79% rename from panther_lights/CMakeLists.txt rename to husarion_ugv_lights/CMakeLists.txt index 070bd8c1..2631fc5e 100644 --- a/panther_lights/CMakeLists.txt +++ b/husarion_ugv_lights/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(panther_lights) +project(husarion_ugv_lights) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -8,9 +8,10 @@ endif() set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater + generate_parameter_library image_transport panther_msgs - panther_utils + husarion_ugv_utils pluginlib rclcpp rclcpp_components @@ -26,11 +27,10 @@ include_directories(include) pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) -add_library( - panther_animation_plugins SHARED src/animation/image_animation.cpp - src/animation/charging_animation.cpp) -ament_target_dependencies(panther_animation_plugins panther_utils pluginlib) -target_link_libraries(panther_animation_plugins png yaml-cpp) +add_library(animation_plugins SHARED src/animation/image_animation.cpp + src/animation/charging_animation.cpp) +ament_target_dependencies(animation_plugins husarion_ugv_utils pluginlib) +target_link_libraries(animation_plugins png yaml-cpp) add_library(lights_driver_node_component SHARED src/lights_driver_node.cpp src/apa102.cpp) @@ -44,6 +44,10 @@ ament_target_dependencies( sensor_msgs std_srvs) +generate_parameter_library(lights_driver_parameters + config/lights_driver_parameters.yaml) +target_link_libraries(lights_driver_node_component lights_driver_parameters) + add_library( lights_controller_node_component SHARED src/lights_controller_node.cpp src/led_components/led_segment.cpp @@ -52,24 +56,28 @@ add_library( ament_target_dependencies( lights_controller_node_component panther_msgs - panther_utils + husarion_ugv_utils pluginlib rclcpp rclcpp_components sensor_msgs) -target_link_libraries(lights_controller_node_component yaml-cpp) + +generate_parameter_library(lights_controller_parameters + config/lights_controller_parameters.yaml) +target_link_libraries(lights_controller_node_component yaml-cpp + lights_controller_parameters) rclcpp_components_register_node( - lights_driver_node_component PLUGIN "panther_lights::LightsDriverNode" + lights_driver_node_component PLUGIN "husarion_ugv_lights::LightsDriverNode" EXECUTABLE lights_driver_node) rclcpp_components_register_node( lights_controller_node_component PLUGIN - "panther_lights::LightsControllerNode" EXECUTABLE lights_controller_node) + "husarion_ugv_lights::LightsControllerNode" EXECUTABLE lights_controller_node) ament_export_targets(export_lights_driver_node_component) install( - TARGETS lights_driver_node_component + TARGETS lights_driver_node_component lights_driver_parameters EXPORT export_lights_driver_node_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -77,15 +85,16 @@ install( ament_export_targets(export_lights_controller_node_component) install( - TARGETS lights_controller_node_component + TARGETS lights_controller_node_component lights_controller_parameters EXPORT export_lights_controller_node_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) -install(TARGETS panther_animation_plugins LIBRARY DESTINATION lib) +install(TARGETS animation_plugins LIBRARY DESTINATION lib) -install(DIRECTORY animations config launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY animations config launch test + DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -99,7 +108,7 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_animation PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_animation panther_utils) + ament_target_dependencies(${PROJECT_NAME}_test_animation husarion_ugv_utils) target_link_libraries(${PROJECT_NAME}_test_animation yaml-cpp) ament_add_gtest( @@ -111,7 +120,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_charging_animation - ament_index_cpp panther_utils pluginlib) + ament_index_cpp husarion_ugv_utils pluginlib) target_link_libraries(${PROJECT_NAME}_test_charging_animation yaml-cpp) ament_add_gtest( @@ -123,7 +132,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_image_animation - ament_index_cpp panther_utils pluginlib) + ament_index_cpp husarion_ugv_utils pluginlib) target_link_libraries(${PROJECT_NAME}_test_image_animation png yaml-cpp) ament_add_gtest( @@ -143,7 +152,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_led_segment pluginlib - panther_utils) + husarion_ugv_utils) target_link_libraries(${PROJECT_NAME}_test_led_segment yaml-cpp) ament_add_gtest( @@ -157,7 +166,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_segment_converter pluginlib - panther_utils) + husarion_ugv_utils) target_link_libraries(${PROJECT_NAME}_test_segment_converter yaml-cpp) ament_add_gtest( @@ -171,7 +180,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_led_animation pluginlib - panther_utils rclcpp) + husarion_ugv_utils rclcpp) target_link_libraries(${PROJECT_NAME}_test_led_animation yaml-cpp) ament_add_gtest( @@ -185,7 +194,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_led_animations_queue pluginlib - panther_utils rclcpp) + husarion_ugv_utils rclcpp) target_link_libraries(${PROJECT_NAME}_test_led_animations_queue yaml-cpp) ament_add_gmock(${PROJECT_NAME}_test_apa102 test/unit/test_apa102.cpp @@ -204,7 +213,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_lights_driver_node - panther_utils) + husarion_ugv_utils) target_link_libraries(${PROJECT_NAME}_test_lights_driver_node lights_driver_node_component) @@ -221,19 +230,19 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_lights_controller_node - panther_utils) + husarion_ugv_utils) target_link_libraries(${PROJECT_NAME}_test_lights_controller_node lights_controller_node_component yaml-cpp) # Integration tests option(TEST_INTEGRATION "Run integration tests" OFF) if(TEST_INTEGRATION) - add_ros_test(test/integration/panther_lights.test.py) + add_ros_test(test/integration/husarion_ugv_lights.test.py) endif() endif() ament_export_include_directories(include) -ament_export_libraries(panther_animation_plugins) +ament_export_libraries(animation_plugins) ament_package() diff --git a/panther_lights/CONFIGURATION.md b/husarion_ugv_lights/CONFIGURATION.md similarity index 80% rename from panther_lights/CONFIGURATION.md rename to husarion_ugv_lights/CONFIGURATION.md index 75f1d434..a4d8d6de 100644 --- a/panther_lights/CONFIGURATION.md +++ b/husarion_ugv_lights/CONFIGURATION.md @@ -1,8 +1,8 @@ -# panther_light +# husarion_ugv_lights ## LED Animations -Basic led configuration is loaded from [`led_config.yaml`](config/led_config.yaml) file. It includes definition of robot panels, virtual segments and default animations. Default animations can be found in the table below: +Basic led configuration is loaded from [`{robot_model}_animations.yaml`](config) file. It includes definition of robot panels, virtual segments and default animations. The default appearance of the animation when looking at the robot from the front or back is as follows: | ID | NAME | PRIORITY | ANIMATION | | :---: | ----------------- | :------: | -------------------------------------------------- | @@ -44,7 +44,7 @@ The `segments_map` section allows creating named groups of segments on which ani The `led_animations` section contains a list with definitions for various animations that can be displayed on the LED segments. Supported keys are: - `animations` [*list*, default: **None**]: definition of animation for each Bumper Lights. Supported keys are: - - `type` [*string*, default **None**]: Specifies the type of animation. Default animation types are: `panther_lights::ImageAnimation`, `panther_lights::ChargingAnimation`. + - `type` [*string*, default **None**]: Specifies the type of animation. Default animation types are: `husarion_ugv_lights::ImageAnimation`, `husarion_ugv_lights::ChargingAnimation`. - `segments` [*string*, default **None**]: Indicates which segment mapping this particular animation applies to (e.g., all, front, rear). - `animation` [*yaml*, default: **None**]: An animation to be displayed on segments. The keys for the configuration of different animation types are explained in detail under the [**Animation Types**](#animation-types) section. - `id` [*int*, default: **None**]: unique ID of an animation. @@ -70,14 +70,14 @@ Basic animation definition. Keys are inherited from the basic **Animation** clas #### ImageAnimation -Animation of type `panther_lights::ImageAnimation`, returns frames to display based on a supplied image. Extends `Animation` with keys: +Animation of type `husarion_ugv_lights::ImageAnimation`, returns frames to display based on a supplied image. Extends `Animation` with keys: - `color` [*int*, default: **None**]: The image is turned into grayscale, and then the color is applied with brightness from the gray image. Values have to be in HEX format. This parameter is not required. - `image` [*string*, default: **None**]: path to an image file. Only global paths are valid. Allows using `$(find ros_package)` syntax. #### ChargingAnimation -Animation of type `panther_lights::ChargingAnimation`, returns frame to display based on `param` value representing Battery percentage. Displays a solid color with a duty cycle proportional to the Battery percentage. The color is changing from red (Battery discharged) to green (Battery fully charged). +Animation of type `husarion_ugv_lights::ChargingAnimation`, returns frame to display based on `param` value representing Battery percentage. Displays a solid color with a duty cycle proportional to the Battery percentage. The color is changing from red (Battery discharged) to green (Battery fully charged). ### Defining Animations @@ -93,10 +93,10 @@ user_animations: name: ANIMATION_1 priority: 2 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find panther_lights)/animations/strip01_red.png + image: $(find husarion_ugv_lights)/animations/panther/strip01_red.png duration: 2 repeat: 2 color: 0xffff00 @@ -106,7 +106,7 @@ user_animations: name: ANIMATION_2 priority: 3 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: image: /animations/custom_image.png @@ -118,10 +118,10 @@ user_animations: name: ANIMATION_3 priority: 3 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find my_custom_animation_package)/animations/custom_image.png + image: $(find custom_pkg)/animations/custom_image.png duration: 3 repeat: 1 @@ -130,30 +130,29 @@ user_animations: name: ANIMATION_4 priority: 3 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: front animation: - image: $(find panther_lights)/animations/triangle01_blue.png + image: $(find custom_pkg)/animations/front_custom_image.png duration: 2 repeat: 2 - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: rear animation: - image: $(find panther_lights)/animations/triangle01_red.png + image: $(find custom_pkg)/animations/rear_custom_image.png duration: 3 repeat: 1 ``` -> [!NOTE] -> ID numbers from 0 to 19 are reserved for system animations. - -> [!NOTE] -> Priority **1** is reserved for crucial system animations. Users can only define animations with priority **2** and **3**. +> [!IMPORTANT] +> +> - ID numbers from 0 to 19 are reserved for system animations. +> - Priority **1** is reserved for crucial system animations. Users can only define animations with priority **2** and **3**. Remember to modify launch command to use user animations: ``` bash -ros2 launch panther_bringup bringup.launch user_animations_file:=/my_awesome_user_animations.yaml +ros2 launch husarion_ugv_bringup bringup.launch user_animations_file:=/my_awesome_user_animations.yaml ``` Test new animations: diff --git a/panther_lights/LIGHTS_API.md b/husarion_ugv_lights/LIGHTS_API.md similarity index 92% rename from panther_lights/LIGHTS_API.md rename to husarion_ugv_lights/LIGHTS_API.md index 4def8a0e..cef272ad 100644 --- a/panther_lights/LIGHTS_API.md +++ b/husarion_ugv_lights/LIGHTS_API.md @@ -35,7 +35,7 @@ Getters: ## Defining a New Animation Type -It is possible to define your own animation type with expected, new behavior. All animation definitions inherit from the basic `Animation` class. Animations are loaded using `pluginlib` and can be defined from any point in your project. All you need is to import `Animation` class from `panther_lights` package and export it as a pluginlib plugin. For more information about creating and managing pluginlib see: [Creating and using plugins (C++)](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html). +It is possible to define your own animation type with expected, new behavior. All animation definitions inherit from the basic `Animation` class. Animations are loaded using `pluginlib` and can be defined from any point in your project. All you need is to import `Animation` class from `husarion_ugv_lights` package and export it as a pluginlib plugin. For more information about creating and managing pluginlib see: [Creating and using plugins (C++)](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html). Frames are processed in ticks with a frequency of `controller_frequency`. It is required to overwrite the `UpdateFrame` method which must return a list representing the frame for a given tick. The advised way is to use the `GetAnimationIteration` (current animation tick) as a time base for animation frames. The length of the frame has to match `num_led`. Each element of the frame represents a color for a single LED in the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. Additional parameters (e.g. image path) can be passed within `animation_description` and processed inside `Initialize` method. See the example below or other animation definitions. @@ -49,7 +49,7 @@ Create a New Animation Type: #include "yaml-cpp/yaml.h" -#include "panther_lights/animation/animation.hpp" +#include "husarion_ugv_lights/animation/animation.hpp" class MyCoolAnimation : public Animation { diff --git a/panther_lights/README.md b/husarion_ugv_lights/README.md similarity index 61% rename from panther_lights/README.md rename to husarion_ugv_lights/README.md index 8c1a7331..f53bf287 100644 --- a/panther_lights/README.md +++ b/husarion_ugv_lights/README.md @@ -1,22 +1,25 @@ -# panther_lights +# husarion_ugv_lights -Package used to control the Husarion Panther Bumper Lights. +Package used to control the Husarion UGV robot's lights. ## Launch files This package contains: -- `lights.launch.py`: Responsible for launching the nodes required to control the Panther Bumper Lights. +- `lights.launch.py`: Responsible for launching the nodes required to control the robot's lights. ## Configuration Files -- [`led_config.yaml`](./config/led_config.yaml): Defines and describes the appearance and parameters of the animations. +- [`{robot_model}_animations.yaml`](./config): Defines and describes the appearance and parameters of the animations for specific robot. +- [`{robot_model}_driver.yaml`](./config): Defines and describes specific hardware configuration for specific robot. +- [`lights_controller_parameters.yaml`](./config/lights_controller_parameters.yaml): Defines parameters for `lights_controller_node`. +- [`lights_driver_parameters.yaml`](./config/lights_driver_parameters.yaml): Defines parameters for `lights_driver_node`. ## ROS Nodes ### LightsControllerNode -This node is of type rclcpp_components is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights. +This node is of type rclcpp_components is responsible for processing animations and publishing frames to `light_driver` node. #### Publishers @@ -29,13 +32,13 @@ This node is of type rclcpp_components is responsible for processing animations #### Parameters +- `~animations_config_path` [*string*, default: **$(find husarion_ugv_lights)/husarion_ugv_lights/config/{robot_model}_animations.yaml**]: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. - `~controller_frequency` [*float*, default: **50.0**]: Frequency [Hz] at which the lights controller node will process animations. -- `~led_config_file` [*string*, default: **$(find panther_lights)/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. -- `~user_led_animations_file` [*string*, default: **None**]: Path to a YAML file with a description of the user defined animations. +- `~user_led_animations_path` [*string*, default: **None**]: Path to a YAML file with a description of the user defined animations. ### LightsDriverNode -This node is of type rclcpp_components is responsible for displaying frames on the Husarion Panther robot's Bumper Lights. +This node is of type rclcpp_components is responsible for displaying frames on the robot's lights. #### Publishers @@ -58,4 +61,5 @@ This node is of type rclcpp_components is responsible for displaying frames on t - `~frame_timeout` [*float*, default: **0.1**]: Time in **[s]** after which an incoming frame will be considered too old. - `~global_brightness` [*float*, default: **1.0**]: LED global brightness. The range between **[0.0, 1.0]**. -- `~num_led` [*int*, default: **46**]: Number of LEDs in a single bumper. +- `~channel_1_num_led` [*int*, default: **46**]: Number of LEDs in the first bumper. +- `~channel_2_num_led` [*int*, default: **46**]: Number of LEDs in the second bumper. diff --git a/husarion_ugv_lights/animations/lynx/battery_critical.png b/husarion_ugv_lights/animations/lynx/battery_critical.png new file mode 100644 index 00000000..e5fc7723 Binary files /dev/null and b/husarion_ugv_lights/animations/lynx/battery_critical.png differ diff --git a/husarion_ugv_lights/animations/lynx/battery_low.png b/husarion_ugv_lights/animations/lynx/battery_low.png new file mode 100644 index 00000000..4507920c Binary files /dev/null and b/husarion_ugv_lights/animations/lynx/battery_low.png differ diff --git a/panther_lights/animations/strip01_purple.png b/husarion_ugv_lights/animations/lynx/strip01_purple.png similarity index 100% rename from panther_lights/animations/strip01_purple.png rename to husarion_ugv_lights/animations/lynx/strip01_purple.png diff --git a/panther_lights/animations/strip01_red.png b/husarion_ugv_lights/animations/lynx/strip01_red.png similarity index 100% rename from panther_lights/animations/strip01_red.png rename to husarion_ugv_lights/animations/lynx/strip01_red.png diff --git a/husarion_ugv_lights/animations/lynx/triangle01_blue.png b/husarion_ugv_lights/animations/lynx/triangle01_blue.png new file mode 100644 index 00000000..e1c907cb Binary files /dev/null and b/husarion_ugv_lights/animations/lynx/triangle01_blue.png differ diff --git a/husarion_ugv_lights/animations/lynx/triangle01_cyan.png b/husarion_ugv_lights/animations/lynx/triangle01_cyan.png new file mode 100644 index 00000000..192a8eef Binary files /dev/null and b/husarion_ugv_lights/animations/lynx/triangle01_cyan.png differ diff --git a/husarion_ugv_lights/animations/lynx/triangle01_green.png b/husarion_ugv_lights/animations/lynx/triangle01_green.png new file mode 100644 index 00000000..bad06bf7 Binary files /dev/null and b/husarion_ugv_lights/animations/lynx/triangle01_green.png differ diff --git a/husarion_ugv_lights/animations/lynx/triangle01_orange.png b/husarion_ugv_lights/animations/lynx/triangle01_orange.png new file mode 100644 index 00000000..89a506af Binary files /dev/null and b/husarion_ugv_lights/animations/lynx/triangle01_orange.png differ diff --git a/husarion_ugv_lights/animations/lynx/triangle01_purple.png b/husarion_ugv_lights/animations/lynx/triangle01_purple.png new file mode 100644 index 00000000..b70f26a3 Binary files /dev/null and b/husarion_ugv_lights/animations/lynx/triangle01_purple.png differ diff --git a/husarion_ugv_lights/animations/lynx/triangle01_red.png b/husarion_ugv_lights/animations/lynx/triangle01_red.png new file mode 100644 index 00000000..7cfd0ce8 Binary files /dev/null and b/husarion_ugv_lights/animations/lynx/triangle01_red.png differ diff --git a/husarion_ugv_lights/animations/lynx/triangle01_yellow.png b/husarion_ugv_lights/animations/lynx/triangle01_yellow.png new file mode 100644 index 00000000..5a1efd57 Binary files /dev/null and b/husarion_ugv_lights/animations/lynx/triangle01_yellow.png differ diff --git a/panther_lights/animations/battery_critical.png b/husarion_ugv_lights/animations/panther/battery_critical.png similarity index 100% rename from panther_lights/animations/battery_critical.png rename to husarion_ugv_lights/animations/panther/battery_critical.png diff --git a/panther_lights/animations/battery_low.png b/husarion_ugv_lights/animations/panther/battery_low.png similarity index 100% rename from panther_lights/animations/battery_low.png rename to husarion_ugv_lights/animations/panther/battery_low.png diff --git a/husarion_ugv_lights/animations/panther/strip01_purple.png b/husarion_ugv_lights/animations/panther/strip01_purple.png new file mode 100644 index 00000000..bf4df0a7 Binary files /dev/null and b/husarion_ugv_lights/animations/panther/strip01_purple.png differ diff --git a/husarion_ugv_lights/animations/panther/strip01_red.png b/husarion_ugv_lights/animations/panther/strip01_red.png new file mode 100644 index 00000000..5886cfe1 Binary files /dev/null and b/husarion_ugv_lights/animations/panther/strip01_red.png differ diff --git a/panther_lights/animations/triangle01_blue.png b/husarion_ugv_lights/animations/panther/triangle01_blue.png similarity index 100% rename from panther_lights/animations/triangle01_blue.png rename to husarion_ugv_lights/animations/panther/triangle01_blue.png diff --git a/panther_lights/animations/triangle01_cyan.png b/husarion_ugv_lights/animations/panther/triangle01_cyan.png similarity index 100% rename from panther_lights/animations/triangle01_cyan.png rename to husarion_ugv_lights/animations/panther/triangle01_cyan.png diff --git a/panther_lights/animations/triangle01_green.png b/husarion_ugv_lights/animations/panther/triangle01_green.png similarity index 100% rename from panther_lights/animations/triangle01_green.png rename to husarion_ugv_lights/animations/panther/triangle01_green.png diff --git a/panther_lights/animations/triangle01_orange.png b/husarion_ugv_lights/animations/panther/triangle01_orange.png similarity index 100% rename from panther_lights/animations/triangle01_orange.png rename to husarion_ugv_lights/animations/panther/triangle01_orange.png diff --git a/panther_lights/animations/triangle01_purple.png b/husarion_ugv_lights/animations/panther/triangle01_purple.png similarity index 100% rename from panther_lights/animations/triangle01_purple.png rename to husarion_ugv_lights/animations/panther/triangle01_purple.png diff --git a/panther_lights/animations/triangle01_red.png b/husarion_ugv_lights/animations/panther/triangle01_red.png similarity index 100% rename from panther_lights/animations/triangle01_red.png rename to husarion_ugv_lights/animations/panther/triangle01_red.png diff --git a/panther_lights/animations/triangle01_yellow.png b/husarion_ugv_lights/animations/panther/triangle01_yellow.png similarity index 100% rename from panther_lights/animations/triangle01_yellow.png rename to husarion_ugv_lights/animations/panther/triangle01_yellow.png diff --git a/husarion_ugv_lights/config/lights_controller_parameters.yaml b/husarion_ugv_lights/config/lights_controller_parameters.yaml new file mode 100644 index 00000000..0994c7b4 --- /dev/null +++ b/husarion_ugv_lights/config/lights_controller_parameters.yaml @@ -0,0 +1,17 @@ +lights_controller: + animations_config_path: + type: string + default_value: "" + description: Path to a YAML file with a description of LED configuration. This file includes definition of robot panels, virtual segments and default animations. + validation: { not_empty<> } + + controller_frequency: + type: double + default_value: 50.0 + description: Frequency in Hz at which the lights controller node will process animations. + validation: { gt<>: 0.0 } + + user_led_animations_path: + type: string + default_value: "" + description: Path to a YAML file with a description of the user defined animations. diff --git a/husarion_ugv_lights/config/lights_driver_parameters.yaml b/husarion_ugv_lights/config/lights_driver_parameters.yaml new file mode 100644 index 00000000..87e89030 --- /dev/null +++ b/husarion_ugv_lights/config/lights_driver_parameters.yaml @@ -0,0 +1,24 @@ +lights_driver: + frame_timeout: + type: double + default_value: 0.1 + description: Time in seconds after which an incoming frame will be considered too old. + validation: { gt<>: 0.0 } + + global_brightness: + type: double + default_value: 1.0 + description: LED global brightness. The range should be between [0.0, 1.0]. + validation: { bounds<>: [0.0, 1.0] } + + channel_1_num_led: + type: int + default_value: 46 + description: Number of LEDs in the first bumper. + validation: { gt<>: 0 } + + channel_2_num_led: + type: int + default_value: 46 + description: Number of LEDs in the second bumper. + validation: { gt<>: 0 } diff --git a/husarion_ugv_lights/config/lynx_animations.yaml b/husarion_ugv_lights/config/lynx_animations.yaml new file mode 100644 index 00000000..c6b0c477 --- /dev/null +++ b/husarion_ugv_lights/config/lynx_animations.yaml @@ -0,0 +1,126 @@ +panels: + - channel: 1 + number_of_leds: 22 + - channel: 2 + number_of_leds: 22 + +segments: + - name: fl + channel: 1 + led_range: 21-11 + - name: rl + channel: 1 + led_range: 0-10 + - name: fr + channel: 2 + led_range: 21-11 + - name: rr + channel: 2 + led_range: 0-10 + +segments_map: + all: [fl, fr, rl, rr] + front: [fl, fr] + rear: [rl, rr] + fl: [fl] + fr: [fr] + rl: [rl] + rr: [rr] + +led_animations: + - id: 0 + name: E_STOP + priority: 3 + animations: + - type: husarion_ugv_lights::ImageAnimation + segments: all + animation: + image: $(find husarion_ugv_lights)/animations/lynx/triangle01_red.png + duration: 2 + + - id: 1 + name: READY + priority: 3 + animations: + - type: husarion_ugv_lights::ImageAnimation + segments: all + animation: + image: $(find husarion_ugv_lights)/animations/lynx/triangle01_green.png + duration: 2 + + - id: 2 + name: ERROR + priority: 1 + animations: + - type: husarion_ugv_lights::ImageAnimation + segments: all + animation: + image: $(find husarion_ugv_lights)/animations/lynx/strip01_red.png + duration: 2 + repeat: 2 + + - id: 3 + name: MANUAL_ACTION + priority: 3 + animations: + - type: husarion_ugv_lights::ImageAnimation + segments: all + animation: + image: $(find husarion_ugv_lights)/animations/lynx/triangle01_blue.png + duration: 3 + + - id: 4 + name: AUTONOMOUS_ACTION + priority: 3 + animations: + - type: husarion_ugv_lights::ImageAnimation + segments: all + animation: + image: $(find husarion_ugv_lights)/animations/lynx/triangle01_orange.png + duration: 3 + + - id: 5 + name: GOAL_ACHIEVED + priority: 2 + animations: + - type: husarion_ugv_lights::ImageAnimation + segments: all + animation: + image: $(find husarion_ugv_lights)/animations/lynx/strip01_purple.png + duration: 3 + repeat: 2 + + - id: 6 + name: LOW_BATTERY + priority: 2 + animations: + - type: husarion_ugv_lights::ImageAnimation + segments: all + animation: + image: $(find husarion_ugv_lights)/animations/lynx/battery_low.png + duration: 2 + repeat: 2 + + - id: 7 + name: CRITICAL_BATTERY + priority: 2 + animations: + - type: husarion_ugv_lights::ImageAnimation + segments: all + animation: + image: $(find husarion_ugv_lights)/animations/lynx/battery_critical.png + duration: 2 + repeat: 2 + + # The BATTERY_STATE animation is no longer supported. + # Animation ID: 8 was intentionally omitted to keep numbering compatibility with ROS 1. + + - id: 9 + name: CHARGING_BATTERY + priority: 3 + animations: + - type: husarion_ugv_lights::ChargingAnimation + segments: all + animation: + duration: 3 + repeat: 2 diff --git a/husarion_ugv_lights/config/lynx_driver.yaml b/husarion_ugv_lights/config/lynx_driver.yaml new file mode 100644 index 00000000..576c6cbe --- /dev/null +++ b/husarion_ugv_lights/config/lynx_driver.yaml @@ -0,0 +1,3 @@ +# In ComposableNode ros__parameters are not supported: +channel_1_num_led: 22 +channel_2_num_led: 22 diff --git a/panther_lights/config/led_config.yaml b/husarion_ugv_lights/config/panther_animations.yaml similarity index 59% rename from panther_lights/config/led_config.yaml rename to husarion_ugv_lights/config/panther_animations.yaml index 4860ad50..fc574749 100644 --- a/panther_lights/config/led_config.yaml +++ b/husarion_ugv_lights/config/panther_animations.yaml @@ -22,61 +22,61 @@ led_animations: name: E_STOP priority: 3 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find panther_lights)/animations/triangle01_red.png + image: $(find husarion_ugv_lights)/animations/panther/triangle01_red.png duration: 2 - id: 1 name: READY priority: 3 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find panther_lights)/animations/triangle01_green.png + image: $(find husarion_ugv_lights)/animations/panther/triangle01_green.png duration: 2 - id: 2 name: ERROR priority: 1 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find panther_lights)/animations/strip01_red.png - duration: 3 + image: $(find husarion_ugv_lights)/animations/panther/strip01_red.png + duration: 2 repeat: 2 - id: 3 name: MANUAL_ACTION priority: 3 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find panther_lights)/animations/triangle01_blue.png + image: $(find husarion_ugv_lights)/animations/panther/triangle01_blue.png duration: 3 - id: 4 name: AUTONOMOUS_ACTION priority: 3 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find panther_lights)/animations/triangle01_orange.png + image: $(find husarion_ugv_lights)/animations/panther/triangle01_orange.png duration: 3 - id: 5 name: GOAL_ACHIEVED priority: 2 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find panther_lights)/animations/strip01_purple.png + image: $(find husarion_ugv_lights)/animations/panther/strip01_purple.png duration: 3 repeat: 2 @@ -84,10 +84,10 @@ led_animations: name: LOW_BATTERY priority: 2 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find panther_lights)/animations/battery_low.png + image: $(find husarion_ugv_lights)/animations/panther/battery_low.png duration: 2 repeat: 2 @@ -95,10 +95,10 @@ led_animations: name: CRITICAL_BATTERY priority: 2 animations: - - type: panther_lights::ImageAnimation + - type: husarion_ugv_lights::ImageAnimation segments: all animation: - image: $(find panther_lights)/animations/battery_critical.png + image: $(find husarion_ugv_lights)/animations/panther/battery_critical.png duration: 2 repeat: 2 @@ -109,7 +109,7 @@ led_animations: name: CHARGING_BATTERY priority: 3 animations: - - type: panther_lights::ChargingAnimation + - type: husarion_ugv_lights::ChargingAnimation segments: all animation: duration: 3 diff --git a/husarion_ugv_lights/config/panther_driver.yaml b/husarion_ugv_lights/config/panther_driver.yaml new file mode 100644 index 00000000..10b4b378 --- /dev/null +++ b/husarion_ugv_lights/config/panther_driver.yaml @@ -0,0 +1,3 @@ +# In ComposableNode ros__parameters are not supported: +channel_1_num_led: 46 +channel_2_num_led: 46 diff --git a/husarion_ugv_lights/config/user_animations.yaml b/husarion_ugv_lights/config/user_animations.yaml new file mode 100644 index 00000000..0d31ca44 --- /dev/null +++ b/husarion_ugv_lights/config/user_animations.yaml @@ -0,0 +1,22 @@ +# This file is a placeholder for user-defined animations for the Husarion UGV +# robots lights system. Users can define their own light animations in this +# YAML file. +# +# For more examples and detailed documentation, please visit: +# https://github.com/husarion/panther_ros/blob/ros2/panther_lights/CONFIGURATION.md#defining-animations + +user_animations: [] + +# Example including a simple custom animation: +# +# user_animations: +# - id: 21 +# name: CUSTOM_IMAGE_ANIMATION +# priority: 3 +# animations: +# - type: panther_lights::ImageAnimation +# segments: all +# animation: +# image: /config/husarion_ugv_lights/animations/example.png +# duration: 3 +# repeat: 1 diff --git a/panther_lights/include/panther_lights/animation/animation.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/animation/animation.hpp similarity index 91% rename from panther_lights/include/panther_lights/animation/animation.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/animation/animation.hpp index 9c237b1d..98c9bf1d 100644 --- a/panther_lights/include/panther_lights/animation/animation.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/animation/animation.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_ANIMATION_ANIMATION_HPP_ -#define PANTHER_LIGHTS_ANIMATION_ANIMATION_HPP_ +#ifndef HUSARION_UGV_LIGHTS_ANIMATION_ANIMATION_HPP_ +#define HUSARION_UGV_LIGHTS_ANIMATION_ANIMATION_HPP_ #include #include @@ -25,9 +25,9 @@ #include "yaml-cpp/yaml.h" -#include "panther_utils/yaml_utils.hpp" +#include "husarion_ugv_utils/yaml_utils.hpp" -namespace panther_lights +namespace husarion_ugv_lights { class Animation @@ -53,12 +53,12 @@ class Animation num_led_ = num_led; frame_ = std::vector(num_led_ * kRGBAColorLen, 0); - auto duration = panther_utils::GetYAMLKeyValue(animation_description, "duration"); + auto duration = husarion_ugv_utils::GetYAMLKeyValue(animation_description, "duration"); if ((duration - std::numeric_limits::epsilon()) <= 0.0) { throw std::out_of_range("Duration has to be positive"); } - loops_ = panther_utils::GetYAMLKeyValue(animation_description, "repeat", 1); + loops_ = husarion_ugv_utils::GetYAMLKeyValue(animation_description, "repeat", 1); if (duration * loops_ > 10.0) { throw std::runtime_error("Animation display duration (duration * repeat) exceeds 10 seconds"); @@ -187,6 +187,6 @@ class Animation std::vector frame_; }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_ANIMATION_ANIMATION_HPP_ +#endif // HUSARION_UGV_LIGHTS_ANIMATION_ANIMATION_HPP_ diff --git a/panther_lights/include/panther_lights/animation/charging_animation.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/animation/charging_animation.hpp similarity index 83% rename from panther_lights/include/panther_lights/animation/charging_animation.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/animation/charging_animation.hpp index 95f7ab7b..2c325bc2 100644 --- a/panther_lights/include/panther_lights/animation/charging_animation.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/animation/charging_animation.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_ -#define PANTHER_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_ +#ifndef HUSARION_UGV_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_ +#define HUSARION_UGV_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_ #include #include @@ -22,9 +22,9 @@ #include "yaml-cpp/yaml.h" -#include "panther_lights/animation/animation.hpp" +#include "husarion_ugv_lights/animation/animation.hpp" -namespace panther_lights +namespace husarion_ugv_lights { class ChargingAnimation : public Animation @@ -57,6 +57,6 @@ class ChargingAnimation : public Animation std::array color_; }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_ +#endif // HUSARION_UGV_LIGHTS_ANIMATION_CHARGING_ANIMATION_HPP_ diff --git a/panther_lights/include/panther_lights/animation/image_animation.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/animation/image_animation.hpp similarity index 88% rename from panther_lights/include/panther_lights/animation/image_animation.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/animation/image_animation.hpp index 31663f95..78506908 100644 --- a/panther_lights/include/panther_lights/animation/image_animation.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/animation/image_animation.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_ -#define PANTHER_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_ +#ifndef HUSARION_UGV_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_ +#define HUSARION_UGV_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_ #include #include @@ -25,11 +25,11 @@ #include "boost/gil.hpp" #include "boost/gil/extension/toolbox/color_spaces/gray_alpha.hpp" -#include "panther_lights/animation/animation.hpp" +#include "husarion_ugv_lights/animation/animation.hpp" namespace gil = boost::gil; -namespace panther_lights +namespace husarion_ugv_lights { class ImageAnimation : public Animation @@ -77,6 +77,6 @@ class ImageAnimation : public Animation gil::rgba8_image_t image_; }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_ +#endif // HUSARION_UGV_LIGHTS_ANIMATION_IMAGE_ANIMATION_HPP_ diff --git a/panther_lights/include/panther_lights/apa102.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/apa102.hpp similarity index 96% rename from panther_lights/include/panther_lights/apa102.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/apa102.hpp index 801a0610..f58ed240 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/apa102.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_APA102_HPP_ -#define PANTHER_LIGHTS_APA102_HPP_ +#ifndef HUSARION_UGV_LIGHTS_APA102_HPP_ +#define HUSARION_UGV_LIGHTS_APA102_HPP_ #include #include @@ -25,7 +25,7 @@ #include #include -namespace panther_lights +namespace husarion_ugv_lights { class SPIDeviceInterface @@ -165,6 +165,6 @@ class APA102 : public APA102Interface const int file_descriptor_; }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_APA102_HPP_ +#endif // HUSARION_UGV_LIGHTS_APA102_HPP_ diff --git a/panther_lights/include/panther_lights/led_components/led_animations_queue.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_animations_queue.hpp similarity index 94% rename from panther_lights/include/panther_lights/led_components/led_animations_queue.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_animations_queue.hpp index e9fd454d..6327bfd5 100644 --- a/panther_lights/include/panther_lights/led_components/led_animations_queue.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_animations_queue.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_ -#define PANTHER_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_ +#ifndef HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_ +#define HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_ #include #include @@ -27,9 +27,9 @@ #include "rclcpp/time.hpp" -#include "panther_lights/led_components/led_segment.hpp" +#include "husarion_ugv_lights/led_components/led_segment.hpp" -namespace panther_lights +namespace husarion_ugv_lights { /** @@ -189,6 +189,6 @@ class LEDAnimationsQueue const std::size_t max_queue_size_; }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_ +#endif // HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_ANIMATIONS_QUEUE_HPP_ diff --git a/panther_lights/include/panther_lights/led_components/led_panel.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_panel.hpp similarity index 85% rename from panther_lights/include/panther_lights/led_components/led_panel.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_panel.hpp index 06d8def5..71719dcb 100644 --- a/panther_lights/include/panther_lights/led_components/led_panel.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_panel.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_ -#define PANTHER_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_ +#ifndef HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_ +#define HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_ #include #include -namespace panther_lights +namespace husarion_ugv_lights { /** @@ -49,6 +49,6 @@ class LEDPanel std::vector frame_; }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_ +#endif // HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_PANEL_HPP_ diff --git a/panther_lights/include/panther_lights/led_components/led_segment.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_segment.hpp similarity index 87% rename from panther_lights/include/panther_lights/led_components/led_segment.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_segment.hpp index 91b03724..5133d947 100644 --- a/panther_lights/include/panther_lights/led_components/led_segment.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/led_segment.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_ -#define PANTHER_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_ +#ifndef HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_ +#define HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_ #include #include @@ -22,9 +22,9 @@ #include "pluginlib/class_loader.hpp" -#include "panther_lights/animation/animation.hpp" +#include "husarion_ugv_lights/animation/animation.hpp" -namespace panther_lights +namespace husarion_ugv_lights { /** @@ -117,8 +117,8 @@ class LEDSegment bool HasAnimation() const { return animation_ || default_animation_; } protected: - std::shared_ptr animation_; - std::shared_ptr default_animation_; + std::shared_ptr animation_; + std::shared_ptr default_animation_; private: const float controller_frequency_; @@ -129,9 +129,9 @@ class LEDSegment std::size_t last_led_iterator_; std::size_t num_led_; - std::shared_ptr> animation_loader_; + std::shared_ptr> animation_loader_; }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_ +#endif // HUSARION_UGV_LIGHTS_LED_COMPONENTS_LED_SEGMENT_HPP_ diff --git a/panther_lights/include/panther_lights/led_components/segment_converter.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/segment_converter.hpp similarity index 70% rename from panther_lights/include/panther_lights/led_components/segment_converter.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/led_components/segment_converter.hpp index fba745cd..1d23cbc8 100644 --- a/panther_lights/include/panther_lights/led_components/segment_converter.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/led_components/segment_converter.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_ -#define PANTHER_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_ +#ifndef HUSARION_UGV_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_ +#define HUSARION_UGV_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_ #include #include #include -#include "panther_lights/led_components/led_panel.hpp" -#include "panther_lights/led_components/led_segment.hpp" +#include "husarion_ugv_lights/led_components/led_panel.hpp" +#include "husarion_ugv_lights/led_components/led_segment.hpp" -namespace panther_lights +namespace husarion_ugv_lights { class SegmentConverter @@ -36,6 +36,6 @@ class SegmentConverter const std::unordered_map> & panels); }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_ +#endif // HUSARION_UGV_LIGHTS_LED_COMPONENTS_SEGMENT_CONVERTER_HPP_ diff --git a/panther_lights/include/panther_lights/lights_controller_node.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp similarity index 88% rename from panther_lights/include/panther_lights/lights_controller_node.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp index ef051bd3..fd01c3ab 100644 --- a/panther_lights/include/panther_lights/lights_controller_node.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_ -#define PANTHER_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_ +#ifndef HUSARION_UGV_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_ +#define HUSARION_UGV_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_ #include #include @@ -27,12 +27,14 @@ #include "panther_msgs/srv/set_led_animation.hpp" -#include "panther_lights/animation/animation.hpp" -#include "panther_lights/led_components/led_animations_queue.hpp" -#include "panther_lights/led_components/segment_converter.hpp" -#include "panther_utils/yaml_utils.hpp" +#include "lights_controller_parameters.hpp" -namespace panther_lights +#include "husarion_ugv_lights/animation/animation.hpp" +#include "husarion_ugv_lights/led_components/led_animations_queue.hpp" +#include "husarion_ugv_lights/led_components/segment_converter.hpp" +#include "husarion_ugv_utils/yaml_utils.hpp" + +namespace husarion_ugv_lights { using ImageMsg = sensor_msgs::msg::Image; @@ -90,9 +92,9 @@ class LightsControllerNode : public rclcpp::Node /** * @brief Adds animations to an unordered map with animations * - * @param user_led_animations_file path to YAML file with user animations description + * @param user_led_animations_path path to YAML file with user animations description */ - void LoadUserAnimations(const std::string & user_led_animations_file); + void LoadUserAnimations(const std::string & user_led_animations_path); /** * @brief Adds animation to an unordered map with animations @@ -148,12 +150,15 @@ class LightsControllerNode : public rclcpp::Node std::unordered_map animations_descriptions_; std::shared_ptr segment_converter_; + std::shared_ptr param_listener_; + lights_controller::Params params_; + rclcpp::Service::SharedPtr set_led_animation_server_; rclcpp::TimerBase::SharedPtr controller_timer_; bool animation_finished_ = true; }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_ +#endif // HUSARION_UGV_LIGHTS_LIGHTS_CONTROLLER_NODE_HPP_ diff --git a/panther_lights/include/panther_lights/lights_driver_node.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp similarity index 90% rename from panther_lights/include/panther_lights/lights_driver_node.hpp rename to husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp index 094dc091..0f015c6c 100644 --- a/panther_lights/include/panther_lights/lights_driver_node.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_LIGHTS_LIGHTS_DRIVER_NODE_HPP_ -#define PANTHER_LIGHTS_LIGHTS_DRIVER_NODE_HPP_ +#ifndef HUSARION_UGV_LIGHTS_LIGHTS_DRIVER_NODE_HPP_ +#define HUSARION_UGV_LIGHTS_LIGHTS_DRIVER_NODE_HPP_ #include #include @@ -26,9 +26,11 @@ #include "panther_msgs/srv/set_led_brightness.hpp" -#include "panther_lights/apa102.hpp" +#include "lights_driver_parameters.hpp" -namespace panther_lights +#include "husarion_ugv_lights/apa102.hpp" + +namespace husarion_ugv_lights { using ImageMsg = sensor_msgs::msg::Image; @@ -83,7 +85,8 @@ class LightsDriverNode : public rclcpp::Node */ void PanelThrottleWarnLog(const std::string panel_name, const std::string message); - int num_led_; + int channel_1_num_led_; + int channel_2_num_led_; double frame_timeout_; bool led_control_granted_; bool led_control_pending_; @@ -119,6 +122,9 @@ class LightsDriverNode : public rclcpp::Node APA102Interface::SharedPtr channel_1_; APA102Interface::SharedPtr channel_2_; + std::shared_ptr param_listener_; + lights_driver::Params params_; + rclcpp::TimerBase::SharedPtr initialization_timer_; rclcpp::Client::SharedPtr enable_led_control_client_; @@ -132,6 +138,6 @@ class LightsDriverNode : public rclcpp::Node diagnostic_updater::Updater diagnostic_updater_; }; -} // namespace panther_lights +} // namespace husarion_ugv_lights -#endif // PANTHER_LIGHTS_LIGHTS_DRIVER_NODE_HPP_ +#endif // HUSARION_UGV_LIGHTS_LIGHTS_DRIVER_NODE_HPP_ diff --git a/panther_lights/launch/lights.launch.py b/husarion_ugv_lights/launch/lights.launch.py similarity index 60% rename from panther_lights/launch/lights.launch.py rename to husarion_ugv_lights/launch/lights.launch.py index f42846f5..27023d82 100644 --- a/panther_lights/launch/lights.launch.py +++ b/husarion_ugv_lights/launch/lights.launch.py @@ -15,6 +15,7 @@ # See the License for the specific language governing permissions and # limitations under the License. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, Shutdown from launch.conditions import UnlessCondition @@ -22,6 +23,7 @@ EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution, + PythonExpression, ) from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -29,12 +31,15 @@ def generate_launch_description(): - led_config_file = LaunchConfiguration("led_config_file") - declare_led_config_file_arg = DeclareLaunchArgument( - "led_config_file", - default_value=PathJoinSubstitution( - [FindPackageShare("panther_lights"), "config", "led_config.yaml"] - ), + robot_model = LaunchConfiguration("robot_model") + husarion_ugv_lights_pkg = FindPackageShare("husarion_ugv_lights") + husarion_ugv_lights_common_dir = PathJoinSubstitution(["/config", "husarion_ugv_lights"]) + animations_config = PythonExpression(["'", robot_model, "_animations.yaml'"]) + + animations_config_path = LaunchConfiguration("animations_config_path") + declare_animations_config_path_arg = DeclareLaunchArgument( + "animations_config_path", + default_value=PathJoinSubstitution([husarion_ugv_lights_pkg, "config", animations_config]), description="Path to a YAML file with a description of led configuration.", ) @@ -45,6 +50,13 @@ def generate_launch_description(): description="Add namespace to all launched nodes.", ) + declare_robot_model_arg = DeclareLaunchArgument( + "robot_model", + default_value=EnvironmentVariable(name="ROBOT_MODEL_NAME", default_value="panther"), + description="Specify robot model", + choices=["lynx", "panther"], + ) + use_sim = LaunchConfiguration("use_sim") declare_use_sim_arg = DeclareLaunchArgument( "use_sim", @@ -52,13 +64,17 @@ def generate_launch_description(): description="Whether simulation is used", ) - user_led_animations_file = LaunchConfiguration("user_led_animations_file") - declare_user_led_animations_file_arg = DeclareLaunchArgument( - "user_led_animations_file", - default_value="", + user_led_animations_path = LaunchConfiguration("user_led_animations_path") + declare_user_led_animations_path_arg = DeclareLaunchArgument( + "user_led_animations_path", + default_value=PathJoinSubstitution( + [husarion_ugv_lights_common_dir, "config", "user_animations.yaml"] + ), description="Path to a YAML file with a description of the user defined animations.", ) + driver_config = PythonExpression(["'", robot_model, "_driver.yaml'"]) + driver_config_path = PathJoinSubstitution([husarion_ugv_lights_pkg, "config", driver_config]) lights_container = ComposableNodeContainer( package="rclcpp_components", name="lights_container", @@ -66,24 +82,25 @@ def generate_launch_description(): executable="component_container", composable_node_descriptions=[ ComposableNode( - package="panther_lights", - plugin="panther_lights::LightsDriverNode", + package="husarion_ugv_lights", + plugin="husarion_ugv_lights::LightsDriverNode", name="lights_driver", namespace=namespace, remappings=[("/diagnostics", "diagnostics")], + parameters=[driver_config_path], extra_arguments=[ {"use_intra_process_comms": True}, ], condition=UnlessCondition(use_sim), ), ComposableNode( - package="panther_lights", - plugin="panther_lights::LightsControllerNode", + package="husarion_ugv_lights", + plugin="husarion_ugv_lights::LightsControllerNode", name="lights_controller", namespace=namespace, parameters=[ - {"led_config_file": led_config_file}, - {"user_led_animations_file": user_led_animations_file}, + {"animations_config_path": animations_config_path}, + {"user_led_animations_path": user_led_animations_path}, ], extra_arguments=[ {"use_intra_process_comms": True}, @@ -95,10 +112,11 @@ def generate_launch_description(): ) actions = [ - declare_led_config_file_arg, + declare_robot_model_arg, # robot_model is used by animations_config_path + declare_animations_config_path_arg, declare_namespace_arg, declare_use_sim_arg, - declare_user_led_animations_file_arg, + declare_user_led_animations_path_arg, lights_container, ] diff --git a/panther_lights/package.xml b/husarion_ugv_lights/package.xml similarity index 86% rename from panther_lights/package.xml rename to husarion_ugv_lights/package.xml index c7a55e77..d6c0e8dd 100644 --- a/panther_lights/package.xml +++ b/husarion_ugv_lights/package.xml @@ -1,9 +1,9 @@ - panther_lights + husarion_ugv_lights 2.1.2 - Package used to control the Husarion Panther Bumper Lights + Package used to control the robot lights Husarion Apache License 2.0 @@ -17,10 +17,11 @@ ament_cmake diagnostic_updater + generate_parameter_library + husarion_ugv_utils image_transport libpng-dev panther_msgs - panther_utils pluginlib rclcpp rclcpp_components diff --git a/husarion_ugv_lights/plugins.xml b/husarion_ugv_lights/plugins.xml new file mode 100644 index 00000000..5b499546 --- /dev/null +++ b/husarion_ugv_lights/plugins.xml @@ -0,0 +1,8 @@ + + + Animation processed from an image + + + Charging animation representing percentage of battery + + diff --git a/panther_lights/src/animation/charging_animation.cpp b/husarion_ugv_lights/src/animation/charging_animation.cpp similarity index 95% rename from panther_lights/src/animation/charging_animation.cpp rename to husarion_ugv_lights/src/animation/charging_animation.cpp index 9a02d6b7..4aeec55d 100644 --- a/panther_lights/src/animation/charging_animation.cpp +++ b/husarion_ugv_lights/src/animation/charging_animation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_lights/animation/charging_animation.hpp" +#include "husarion_ugv_lights/animation/charging_animation.hpp" #include #include @@ -25,7 +25,7 @@ #include "yaml-cpp/yaml.h" -namespace panther_lights +namespace husarion_ugv_lights { void ChargingAnimation::Initialize( @@ -153,8 +153,8 @@ std::vector ChargingAnimation::CreateRGBAFrame( return frame; } -} // namespace panther_lights +} // namespace husarion_ugv_lights #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(panther_lights::ChargingAnimation, panther_lights::Animation) +PLUGINLIB_EXPORT_CLASS(husarion_ugv_lights::ChargingAnimation, husarion_ugv_lights::Animation) diff --git a/panther_lights/src/animation/image_animation.cpp b/husarion_ugv_lights/src/animation/image_animation.cpp similarity index 93% rename from panther_lights/src/animation/image_animation.cpp rename to husarion_ugv_lights/src/animation/image_animation.cpp index 5cc68f08..ab7d9cd9 100644 --- a/panther_lights/src/animation/image_animation.cpp +++ b/husarion_ugv_lights/src/animation/image_animation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_lights/animation/image_animation.hpp" +#include "husarion_ugv_lights/animation/image_animation.hpp" #include #include @@ -30,9 +30,9 @@ #include "boost/gil/extension/numeric/resample.hpp" #include "boost/gil/extension/numeric/sampler.hpp" -#include "panther_utils/yaml_utils.hpp" +#include "husarion_ugv_utils/yaml_utils.hpp" -namespace panther_lights +namespace husarion_ugv_lights { void ImageAnimation::Initialize( @@ -41,8 +41,8 @@ void ImageAnimation::Initialize( { Animation::Initialize(animation_description, num_led, controller_frequency); - const auto image_path = - ParseImagePath(panther_utils::GetYAMLKeyValue(animation_description, "image")); + const auto image_path = ParseImagePath( + husarion_ugv_utils::GetYAMLKeyValue(animation_description, "image")); gil::rgba8_image_t base_image; gil::read_and_convert_image(std::string(image_path), base_image, gil::png_tag()); image_ = RGBAImageResize(base_image, this->GetNumberOfLeds(), this->GetAnimationLength()); @@ -156,8 +156,8 @@ void ImageAnimation::GreyImageNormalizeBrightness(gil::gray_alpha8_image_t & ima }); } -} // namespace panther_lights +} // namespace husarion_ugv_lights #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(panther_lights::ImageAnimation, panther_lights::Animation) +PLUGINLIB_EXPORT_CLASS(husarion_ugv_lights::ImageAnimation, husarion_ugv_lights::Animation) diff --git a/panther_lights/src/apa102.cpp b/husarion_ugv_lights/src/apa102.cpp similarity index 97% rename from panther_lights/src/apa102.cpp rename to husarion_ugv_lights/src/apa102.cpp index 8a2a70aa..df35f857 100644 --- a/panther_lights/src/apa102.cpp +++ b/husarion_ugv_lights/src/apa102.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_lights/apa102.hpp" +#include "husarion_ugv_lights/apa102.hpp" #include #include @@ -22,7 +22,7 @@ #include #include -namespace panther_lights +namespace husarion_ugv_lights { APA102::APA102( @@ -126,4 +126,4 @@ void APA102::SPISendBuffer(const std::vector & buffer) const } } -} // namespace panther_lights +} // namespace husarion_ugv_lights diff --git a/panther_lights/src/led_components/led_animations_queue.cpp b/husarion_ugv_lights/src/led_components/led_animations_queue.cpp similarity index 96% rename from panther_lights/src/led_components/led_animations_queue.cpp rename to husarion_ugv_lights/src/led_components/led_animations_queue.cpp index 0d295ab0..6a0aa04f 100644 --- a/panther_lights/src/led_components/led_animations_queue.cpp +++ b/husarion_ugv_lights/src/led_components/led_animations_queue.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_lights/led_components/led_animations_queue.hpp" +#include "husarion_ugv_lights/led_components/led_animations_queue.hpp" #include #include @@ -23,7 +23,7 @@ #include "rclcpp/logging.hpp" #include "rclcpp/time.hpp" -namespace panther_lights +namespace husarion_ugv_lights { LEDAnimation::LEDAnimation( @@ -148,4 +148,4 @@ bool LEDAnimationsQueue::HasAnimation(const std::shared_ptr & anim return std::find(queue_.begin(), queue_.end(), animation) != queue_.end(); } -} // namespace panther_lights +} // namespace husarion_ugv_lights diff --git a/panther_lights/src/led_components/led_panel.cpp b/husarion_ugv_lights/src/led_components/led_panel.cpp similarity index 93% rename from panther_lights/src/led_components/led_panel.cpp rename to husarion_ugv_lights/src/led_components/led_panel.cpp index 46820f17..6107e1ea 100644 --- a/panther_lights/src/led_components/led_panel.cpp +++ b/husarion_ugv_lights/src/led_components/led_panel.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_lights/led_components/led_panel.hpp" +#include "husarion_ugv_lights/led_components/led_panel.hpp" #include #include #include #include -namespace panther_lights +namespace husarion_ugv_lights { LEDPanel::LEDPanel(const std::size_t num_led) : num_led_(num_led) @@ -50,4 +50,4 @@ void LEDPanel::UpdateFrame( std::copy(values.begin(), values.end(), frame_.begin() + iterator_first); } -} // namespace panther_lights +} // namespace husarion_ugv_lights diff --git a/panther_lights/src/led_components/led_segment.cpp b/husarion_ugv_lights/src/led_components/led_segment.cpp similarity index 86% rename from panther_lights/src/led_components/led_segment.cpp rename to husarion_ugv_lights/src/led_components/led_segment.cpp index bd4eaa98..ef82263d 100644 --- a/panther_lights/src/led_components/led_segment.cpp +++ b/husarion_ugv_lights/src/led_components/led_segment.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_lights/led_components/led_segment.hpp" +#include "husarion_ugv_lights/led_components/led_segment.hpp" #include #include @@ -20,17 +20,17 @@ #include "yaml-cpp/yaml.h" -#include "panther_lights/animation/animation.hpp" -#include "panther_utils/yaml_utils.hpp" +#include "husarion_ugv_lights/animation/animation.hpp" +#include "husarion_ugv_utils/yaml_utils.hpp" -namespace panther_lights +namespace husarion_ugv_lights { LEDSegment::LEDSegment(const YAML::Node & segment_description, const float controller_frequency) : controller_frequency_(controller_frequency) { - channel_ = panther_utils::GetYAMLKeyValue(segment_description, "channel"); - const auto led_range = panther_utils::GetYAMLKeyValue( + channel_ = husarion_ugv_utils::GetYAMLKeyValue(segment_description, "channel"); + const auto led_range = husarion_ugv_utils::GetYAMLKeyValue( segment_description, "led_range"); const std::size_t split_char = led_range.find('-'); @@ -52,8 +52,8 @@ LEDSegment::LEDSegment(const YAML::Node & segment_description, const float contr num_led_ = std::abs(int(last_led_iterator_ - first_led_iterator_)) + 1; - animation_loader_ = std::make_shared>( - "panther_lights", "panther_lights::Animation"); + animation_loader_ = std::make_shared>( + "husarion_ugv_lights", "husarion_ugv_lights::Animation"); } LEDSegment::~LEDSegment() @@ -68,7 +68,7 @@ void LEDSegment::SetAnimation( const std::string & type, const YAML::Node & animation_description, const bool repeating, const std::string & param) { - std::shared_ptr animation; + std::shared_ptr animation; try { animation = animation_loader_->createSharedInstance(type); @@ -107,7 +107,7 @@ void LEDSegment::UpdateAnimation() animation_finished_ = true; } - std::shared_ptr animation_to_update = + std::shared_ptr animation_to_update = animation_finished_ && default_animation_ ? default_animation_ : animation_; if (animation_finished_ && default_animation_ && default_animation_->IsFinished()) { @@ -167,4 +167,4 @@ std::size_t LEDSegment::GetFirstLEDPosition() const return (invert_led_order_ ? last_led_iterator_ : first_led_iterator_) * Animation::kRGBAColorLen; } -} // namespace panther_lights +} // namespace husarion_ugv_lights diff --git a/panther_lights/src/led_components/segment_converter.cpp b/husarion_ugv_lights/src/led_components/segment_converter.cpp similarity index 85% rename from panther_lights/src/led_components/segment_converter.cpp rename to husarion_ugv_lights/src/led_components/segment_converter.cpp index fb1f54dd..b5ecf372 100644 --- a/panther_lights/src/led_components/segment_converter.cpp +++ b/husarion_ugv_lights/src/led_components/segment_converter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_lights/led_components/segment_converter.hpp" +#include "husarion_ugv_lights/led_components/segment_converter.hpp" #include #include @@ -20,10 +20,10 @@ #include #include -#include "panther_lights/led_components/led_panel.hpp" -#include "panther_lights/led_components/led_segment.hpp" +#include "husarion_ugv_lights/led_components/led_panel.hpp" +#include "husarion_ugv_lights/led_components/led_segment.hpp" -namespace panther_lights +namespace husarion_ugv_lights { void SegmentConverter::Convert( @@ -52,4 +52,4 @@ void SegmentConverter::Convert( } } -} // namespace panther_lights +} // namespace husarion_ugv_lights diff --git a/panther_lights/src/lights_controller_node.cpp b/husarion_ugv_lights/src/lights_controller_node.cpp similarity index 81% rename from panther_lights/src/lights_controller_node.cpp rename to husarion_ugv_lights/src/lights_controller_node.cpp index fb2d461f..9306ff0c 100644 --- a/panther_lights/src/lights_controller_node.cpp +++ b/husarion_ugv_lights/src/lights_controller_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_lights/lights_controller_node.hpp" +#include "husarion_ugv_lights/lights_controller_node.hpp" #include #include @@ -31,14 +31,16 @@ #include "panther_msgs/srv/set_led_animation.hpp" -#include "panther_lights/led_components/led_animations_queue.hpp" -#include "panther_lights/led_components/led_panel.hpp" -#include "panther_lights/led_components/led_segment.hpp" -#include "panther_lights/led_components/segment_converter.hpp" -#include "panther_utils/ros_utils.hpp" -#include "panther_utils/yaml_utils.hpp" +#include "lights_controller_parameters.hpp" -namespace panther_lights +#include "husarion_ugv_lights/led_components/led_animations_queue.hpp" +#include "husarion_ugv_lights/led_components/led_panel.hpp" +#include "husarion_ugv_lights/led_components/led_segment.hpp" +#include "husarion_ugv_lights/led_components/segment_converter.hpp" +#include "husarion_ugv_utils/ros_utils.hpp" +#include "husarion_ugv_utils/yaml_utils.hpp" + +namespace husarion_ugv_lights { LightsControllerNode::LightsControllerNode(const rclcpp::NodeOptions & options) @@ -48,23 +50,23 @@ LightsControllerNode::LightsControllerNode(const rclcpp::NodeOptions & options) using namespace std::placeholders; - this->declare_parameter("led_config_file"); - this->declare_parameter("user_led_animations_file", ""); - this->declare_parameter("controller_freq", 50.0); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); - const auto led_config_file = this->get_parameter("led_config_file").as_string(); - const auto user_led_animations_file = this->get_parameter("user_led_animations_file").as_string(); - const float controller_freq = this->get_parameter("controller_freq").as_double(); + const auto animations_config_path = this->params_.animations_config_path; + const auto user_led_animations_path = this->params_.user_led_animations_path; + const float controller_freq = static_cast(this->params_.controller_frequency); - YAML::Node led_config_desc = YAML::LoadFile(led_config_file); + YAML::Node led_config_desc = YAML::LoadFile(animations_config_path); InitializeLEDPanels(led_config_desc["panels"]); InitializeLEDSegments(led_config_desc["segments"], controller_freq); InitializeLEDSegmentsMap(led_config_desc["segments_map"]); LoadDefaultAnimations(led_config_desc["led_animations"]); - if (user_led_animations_file != "") { - LoadUserAnimations(user_led_animations_file); + if (user_led_animations_path != "") { + LoadUserAnimations(user_led_animations_path); } segment_converter_ = std::make_shared(); @@ -86,8 +88,8 @@ void LightsControllerNode::InitializeLEDPanels(const YAML::Node & panels_descrip RCLCPP_DEBUG(this->get_logger(), "Initializing LED panels."); for (auto & panel : panels_description.as>()) { - const auto channel = panther_utils::GetYAMLKeyValue(panel, "channel"); - const auto number_of_leds = panther_utils::GetYAMLKeyValue( + const auto channel = husarion_ugv_utils::GetYAMLKeyValue(panel, "channel"); + const auto number_of_leds = husarion_ugv_utils::GetYAMLKeyValue( panel, "number_of_leds"); const auto result = led_panels_.emplace(channel, std::make_unique(number_of_leds)); @@ -115,7 +117,7 @@ void LightsControllerNode::InitializeLEDSegments( RCLCPP_DEBUG(this->get_logger(), "Initializing LED segments."); for (auto & segment : segments_description.as>()) { - const auto segment_name = panther_utils::GetYAMLKeyValue(segment, "name"); + const auto segment_name = husarion_ugv_utils::GetYAMLKeyValue(segment, "name"); try { const auto result = segments_.emplace( @@ -159,23 +161,23 @@ void LightsControllerNode::LoadDefaultAnimations(const YAML::Node & animations_d RCLCPP_INFO(this->get_logger(), "Loaded default animations."); } -void LightsControllerNode::LoadUserAnimations(const std::string & user_led_animations_file) +void LightsControllerNode::LoadUserAnimations(const std::string & user_led_animations_path) { RCLCPP_DEBUG(this->get_logger(), "Loading user's animations."); try { - YAML::Node user_led_animations = YAML::LoadFile(user_led_animations_file); - auto user_animations = panther_utils::GetYAMLKeyValue>( + YAML::Node user_led_animations = YAML::LoadFile(user_led_animations_path); + auto user_animations = husarion_ugv_utils::GetYAMLKeyValue>( user_led_animations, "user_animations"); for (auto & animation_description : user_animations) { try { - auto id = panther_utils::GetYAMLKeyValue(animation_description, "id"); + auto id = husarion_ugv_utils::GetYAMLKeyValue(animation_description, "id"); if (id < 20) { throw std::runtime_error("Animation ID must be greater than 19."); } - auto priority = panther_utils::GetYAMLKeyValue( + auto priority = husarion_ugv_utils::GetYAMLKeyValue( animation_description, "priority", LEDAnimation::kDefaultPriority); if (priority == 1) { throw std::runtime_error("User animation can not have priority 1."); @@ -199,13 +201,13 @@ void LightsControllerNode::LoadAnimation(const YAML::Node & animation_descriptio LEDAnimationDescription led_animation_desc; try { - led_animation_desc.id = panther_utils::GetYAMLKeyValue( + led_animation_desc.id = husarion_ugv_utils::GetYAMLKeyValue( animation_description, "id"); - led_animation_desc.name = panther_utils::GetYAMLKeyValue( + led_animation_desc.name = husarion_ugv_utils::GetYAMLKeyValue( animation_description, "name", "ANIMATION_" + std::to_string(led_animation_desc.id)); - led_animation_desc.priority = panther_utils::GetYAMLKeyValue( + led_animation_desc.priority = husarion_ugv_utils::GetYAMLKeyValue( animation_description, "priority", LEDAnimation::kDefaultPriority); - led_animation_desc.timeout = panther_utils::GetYAMLKeyValue( + led_animation_desc.timeout = husarion_ugv_utils::GetYAMLKeyValue( animation_description, "timeout", LEDAnimation::kDefaultTimeout); if ( @@ -215,14 +217,15 @@ void LightsControllerNode::LoadAnimation(const YAML::Node & animation_descriptio throw std::runtime_error("Invalid LED animation priority."); } - auto animations = panther_utils::GetYAMLKeyValue>( + auto animations = husarion_ugv_utils::GetYAMLKeyValue>( animation_description, "animations"); for (auto & animation : animations) { AnimationDescription animation_desc; - animation_desc.type = panther_utils::GetYAMLKeyValue(animation, "type"); - animation_desc.animation = panther_utils::GetYAMLKeyValue(animation, "animation"); + animation_desc.type = husarion_ugv_utils::GetYAMLKeyValue(animation, "type"); + animation_desc.animation = husarion_ugv_utils::GetYAMLKeyValue( + animation, "animation"); - auto segments_group = panther_utils::GetYAMLKeyValue(animation, "segments"); + auto segments_group = husarion_ugv_utils::GetYAMLKeyValue(animation, "segments"); animation_desc.segments = segments_map_.at(segments_group); led_animation_desc.animations.push_back(animation_desc); @@ -258,7 +261,7 @@ void LightsControllerNode::PublishPanelFrame(const std::size_t channel) const auto number_of_leds = panel->GetNumberOfLeds(); ImageMsg::UniquePtr image(new ImageMsg); - image->header.frame_id = panther_utils::ros::AddNamespaceToFrameID( + image->header.frame_id = husarion_ugv_utils::ros::AddNamespaceToFrameID( "lights_channel_" + std::to_string(channel) + "_link", std::string(this->get_namespace())); image->header.stamp = this->get_clock()->now(); image->encoding = "rgba8"; @@ -370,7 +373,7 @@ void LightsControllerNode::SetLEDAnimation(const std::shared_ptr & current_animation_ = std::move(led_animation); } -} // namespace panther_lights +} // namespace husarion_ugv_lights #include -RCLCPP_COMPONENTS_REGISTER_NODE(panther_lights::LightsControllerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(husarion_ugv_lights::LightsControllerNode) diff --git a/panther_lights/src/lights_driver_node.cpp b/husarion_ugv_lights/src/lights_driver_node.cpp similarity index 90% rename from panther_lights/src/lights_driver_node.cpp rename to husarion_ugv_lights/src/lights_driver_node.cpp index e014695a..ad502100 100644 --- a/panther_lights/src/lights_driver_node.cpp +++ b/husarion_ugv_lights/src/lights_driver_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_lights/lights_driver_node.hpp" +#include "husarion_ugv_lights/lights_driver_node.hpp" #include #include @@ -31,9 +31,9 @@ #include "panther_msgs/srv/set_led_brightness.hpp" -#include "panther_lights/apa102.hpp" +#include "husarion_ugv_lights/apa102.hpp" -namespace panther_lights +namespace husarion_ugv_lights { using std::placeholders::_1; @@ -52,14 +52,15 @@ LightsDriverNode::LightsDriverNode(const rclcpp::NodeOptions & options) rclcpp::on_shutdown(std::bind(&LightsDriverNode::OnShutdown, this)); - this->declare_parameter("global_brightness", 1.0); - this->declare_parameter("frame_timeout", 0.1); - this->declare_parameter("num_led", 46); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); - frame_timeout_ = this->get_parameter("frame_timeout").as_double(); - num_led_ = this->get_parameter("num_led").as_int(); + frame_timeout_ = this->params_.frame_timeout; + channel_1_num_led_ = this->params_.channel_1_num_led; + channel_2_num_led_ = this->params_.channel_2_num_led; - const float global_brightness = this->get_parameter("global_brightness").as_double(); + const float global_brightness = this->params_.global_brightness; channel_1_->SetGlobalBrightness(global_brightness); channel_2_->SetGlobalBrightness(global_brightness); @@ -133,8 +134,8 @@ void LightsDriverNode::InitializationTimerCB() void LightsDriverNode::ClearLEDs() { - channel_1_->SetPanel(std::vector(num_led_ * 4, 0)); - channel_2_->SetPanel(std::vector(num_led_ * 4, 0)); + channel_1_->SetPanel(std::vector(channel_1_num_led_ * 4, 0)); + channel_2_->SetPanel(std::vector(channel_2_num_led_ * 4, 0)); } void LightsDriverNode::ToggleLEDControl(const bool enable) @@ -211,7 +212,9 @@ void LightsDriverNode::FrameCB( message = "Incorrect image encoding ('" + msg->encoding + "')"; } else if (msg->height != 1) { message = "Incorrect image height " + std::to_string(msg->height); - } else if (msg->width != (std::uint32_t)num_led_) { + } else if ( + msg->width != + (std::uint32_t)(panel_name == "channel_1" ? channel_1_num_led_ : channel_2_num_led_)) { message = "Incorrect image width " + std::to_string(msg->width); } @@ -276,7 +279,7 @@ void LightsDriverNode::DiagnoseLights(diagnostic_updater::DiagnosticStatusWrappe status.summary(error_level, message); } -} // namespace panther_lights +} // namespace husarion_ugv_lights #include -RCLCPP_COMPONENTS_REGISTER_NODE(panther_lights::LightsDriverNode) +RCLCPP_COMPONENTS_REGISTER_NODE(husarion_ugv_lights::LightsDriverNode) diff --git a/husarion_ugv_lights/test/files/animation.png b/husarion_ugv_lights/test/files/animation.png new file mode 100644 index 00000000..5886cfe1 Binary files /dev/null and b/husarion_ugv_lights/test/files/animation.png differ diff --git a/panther_lights/test/integration/panther_lights.test.py b/husarion_ugv_lights/test/integration/husarion_ugv_lights.test.py similarity index 91% rename from panther_lights/test/integration/panther_lights.test.py rename to husarion_ugv_lights/test/integration/husarion_ugv_lights.test.py index fafd6bf1..906adb4b 100644 --- a/panther_lights/test/integration/panther_lights.test.py +++ b/husarion_ugv_lights/test/integration/husarion_ugv_lights.test.py @@ -17,9 +17,9 @@ import unittest +import husarion_ugv_utils.integration_test_utils as test_utils import launch import launch_testing -import panther_utils.integration_test_utils as test_utils import rclpy import rclpy.qos from diagnostic_msgs.msg import DiagnosticArray @@ -36,18 +36,21 @@ def generate_test_description(): - led_config_file = ( - PathJoinSubstitution([FindPackageShare("panther_lights"), "config", "led_config.yaml"]), + # TODO: Should be possibility to launch integration test for specific robot + animations_config_path = ( + PathJoinSubstitution( + [FindPackageShare("husarion_ugv_lights"), "config", "panther_animations.yaml"] + ), ) lights_controller_node = Node( - package="panther_lights", + package="husarion_ugv_lights", executable="lights_controller_node", - parameters=[{"led_config_file": led_config_file}], + parameters=[{"animations_config_path": animations_config_path}], ) lights_driver_node = Node( - package="panther_lights", + package="husarion_ugv_lights", executable="lights_driver_node", ) diff --git a/panther_lights/test/unit/animation/test_animation.cpp b/husarion_ugv_lights/test/unit/animation/test_animation.cpp similarity index 98% rename from panther_lights/test/unit/animation/test_animation.cpp rename to husarion_ugv_lights/test/unit/animation/test_animation.cpp index fb85d6a9..11e1f1bb 100644 --- a/panther_lights/test/unit/animation/test_animation.cpp +++ b/husarion_ugv_lights/test/unit/animation/test_animation.cpp @@ -20,9 +20,9 @@ #include "gtest/gtest.h" #include "yaml-cpp/yaml.h" -#include "panther_lights/animation/animation.hpp" +#include "husarion_ugv_lights/animation/animation.hpp" -class AnimationWrapper : public panther_lights::Animation +class AnimationWrapper : public husarion_ugv_lights::Animation { public: AnimationWrapper() {} diff --git a/panther_lights/test/unit/animation/test_charging_animation.cpp b/husarion_ugv_lights/test/unit/animation/test_charging_animation.cpp similarity index 97% rename from panther_lights/test/unit/animation/test_charging_animation.cpp rename to husarion_ugv_lights/test/unit/animation/test_charging_animation.cpp index 9a1c6d6a..b95eff58 100644 --- a/panther_lights/test/unit/animation/test_charging_animation.cpp +++ b/husarion_ugv_lights/test/unit/animation/test_charging_animation.cpp @@ -21,9 +21,9 @@ #include "gtest/gtest.h" #include "yaml-cpp/yaml.h" -#include "panther_lights/animation/charging_animation.hpp" +#include "husarion_ugv_lights/animation/charging_animation.hpp" -class ChargingAnimationWrapper : public panther_lights::ChargingAnimation +class ChargingAnimationWrapper : public husarion_ugv_lights::ChargingAnimation { public: ChargingAnimationWrapper() {} diff --git a/panther_lights/test/unit/animation/test_image_animation.cpp b/husarion_ugv_lights/test/unit/animation/test_image_animation.cpp similarity index 93% rename from panther_lights/test/unit/animation/test_image_animation.cpp rename to husarion_ugv_lights/test/unit/animation/test_image_animation.cpp index 9437bae1..5fdb993c 100644 --- a/panther_lights/test/unit/animation/test_image_animation.cpp +++ b/husarion_ugv_lights/test/unit/animation/test_image_animation.cpp @@ -24,11 +24,11 @@ #include "gtest/gtest.h" #include "yaml-cpp/yaml.h" -#include "panther_lights/animation/image_animation.hpp" +#include "husarion_ugv_lights/animation/image_animation.hpp" namespace gil = boost::gil; -class ImageAnimationWrapper : public panther_lights::ImageAnimation +class ImageAnimationWrapper : public husarion_ugv_lights::ImageAnimation { public: ImageAnimationWrapper() {} @@ -94,21 +94,21 @@ TEST_F(TestImageAnimation, ParseImagePath) EXPECT_EQ(this->test_image_path, animation_->ParseImagePath(this->test_image_path)); // test ROS package path - image_path = "$(find invalid_ros_package)/animations/strip01_red.png"; + image_path = "$(find invalid_ros_package)/test/files/animation.png"; EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); // invalid substitution - image_path = "$(fin panther_lights)/animations/strip01_red.png"; + image_path = "$(fin husarion_ugv_lights)/test/files/animation.png"; EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); - image_path = "$(find panther_lights/animations/strip01_red.png"; + image_path = "$(find husarion_ugv_lights/test/files/animation.png"; EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); // following ones may not work if ROS package is not build or sourced - image_path = "$(find panther_lights)/animations/strip01_red.png"; + image_path = "$(find husarion_ugv_lights)/test/files/animation.png"; EXPECT_NO_THROW(animation_->ParseImagePath(image_path)); // multiple spaces after find syntax - image_path = "$(find panther_lights)/animations/strip01_red.png"; + image_path = "$(find husarion_ugv_lights)/test/files/animation.png"; EXPECT_NO_THROW(animation_->ParseImagePath(image_path)); } diff --git a/panther_lights/test/unit/led_components/test_led_animation.cpp b/husarion_ugv_lights/test/unit/led_components/test_led_animation.cpp similarity index 79% rename from panther_lights/test/unit/led_components/test_led_animation.cpp rename to husarion_ugv_lights/test/unit/led_components/test_led_animation.cpp index 64dd9b02..67173da0 100644 --- a/panther_lights/test/unit/led_components/test_led_animation.cpp +++ b/husarion_ugv_lights/test/unit/led_components/test_led_animation.cpp @@ -22,8 +22,8 @@ #include "rclcpp/time.hpp" -#include "panther_lights/led_components/led_animations_queue.hpp" -#include "panther_lights/led_components/led_segment.hpp" +#include "husarion_ugv_lights/led_components/led_animations_queue.hpp" +#include "husarion_ugv_lights/led_components/led_segment.hpp" class TestLEDAnimation : public testing::Test { @@ -37,8 +37,8 @@ class TestLEDAnimation : public testing::Test static constexpr char kTestSegmentName1[] = "segment_1"; static constexpr char kTestSegmentName2[] = "segment_2"; - std::shared_ptr led_anim_; - std::unordered_map> segments_; + std::shared_ptr led_anim_; + std::unordered_map> segments_; }; TestLEDAnimation::TestLEDAnimation() @@ -46,24 +46,24 @@ TestLEDAnimation::TestLEDAnimation() auto segment_1_desc = YAML::Load("{channel: 1, led_range: 0-10}"); auto segment_2_desc = YAML::Load("{channel: 2, led_range: 0-10}"); segments_.emplace( - kTestSegmentName1, std::make_shared(segment_1_desc, 50.0)); + kTestSegmentName1, std::make_shared(segment_1_desc, 50.0)); segments_.emplace( - kTestSegmentName2, std::make_shared(segment_2_desc, 50.0)); + kTestSegmentName2, std::make_shared(segment_2_desc, 50.0)); - panther_lights::AnimationDescription anim_desc; + husarion_ugv_lights::AnimationDescription anim_desc; anim_desc.segments = {kTestSegmentName1, kTestSegmentName2}; - anim_desc.type = "panther_lights::ImageAnimation"; + anim_desc.type = "husarion_ugv_lights::ImageAnimation"; anim_desc.animation = - YAML::Load("{image: $(find panther_lights)/animations/triangle01_red.png, duration: 2.0}"); + YAML::Load("{image: $(find husarion_ugv_lights)/test/files/animation.png, duration: 2.0}"); - panther_lights::LEDAnimationDescription led_anim_desc; + husarion_ugv_lights::LEDAnimationDescription led_anim_desc; led_anim_desc.id = 0; led_anim_desc.name = "TEST"; led_anim_desc.priority = 1; led_anim_desc.timeout = 10.0; led_anim_desc.animations = {anim_desc}; - led_anim_ = std::make_shared( + led_anim_ = std::make_shared( led_anim_desc, segments_, rclcpp::Time(0)); } @@ -80,16 +80,16 @@ void TestLEDAnimation::SetSegmentAnimations() TEST(TestLEDAnimationInitialization, InvalidSegmentName) { - std::unordered_map> segments; + std::unordered_map> segments; - panther_lights::AnimationDescription anim_desc; + husarion_ugv_lights::AnimationDescription anim_desc; anim_desc.segments = {"invalid_segment"}; - panther_lights::LEDAnimationDescription led_anim_desc; + husarion_ugv_lights::LEDAnimationDescription led_anim_desc; led_anim_desc.animations = {anim_desc}; EXPECT_THROW( - std::make_shared(led_anim_desc, segments, rclcpp::Time(0)), + std::make_shared(led_anim_desc, segments, rclcpp::Time(0)), std::runtime_error); } diff --git a/panther_lights/test/unit/led_components/test_led_animations_queue.cpp b/husarion_ugv_lights/test/unit/led_components/test_led_animations_queue.cpp similarity index 66% rename from panther_lights/test/unit/led_components/test_led_animations_queue.cpp rename to husarion_ugv_lights/test/unit/led_components/test_led_animations_queue.cpp index bdd9386d..d7d8c04a 100644 --- a/panther_lights/test/unit/led_components/test_led_animations_queue.cpp +++ b/husarion_ugv_lights/test/unit/led_components/test_led_animations_queue.cpp @@ -22,8 +22,8 @@ #include "rclcpp/time.hpp" -#include "panther_lights/led_components/led_animations_queue.hpp" -#include "panther_lights/led_components/led_segment.hpp" +#include "husarion_ugv_lights/led_components/led_animations_queue.hpp" +#include "husarion_ugv_lights/led_components/led_segment.hpp" class TestLEDAnimationsQueue : public testing::Test { @@ -31,13 +31,13 @@ class TestLEDAnimationsQueue : public testing::Test TestLEDAnimationsQueue(); ~TestLEDAnimationsQueue() {} - panther_lights::LEDAnimation CreateLEDAnimation( + husarion_ugv_lights::LEDAnimation CreateLEDAnimation( const std::string & name, const std::uint8_t priority, const rclcpp::Time & init_time = rclcpp::Time(0)); protected: - std::shared_ptr led_anim_queue_; - std::unordered_map> segments_; + std::shared_ptr led_anim_queue_; + std::unordered_map> segments_; const std::size_t max_queue_size_ = 5; }; @@ -47,35 +47,36 @@ TestLEDAnimationsQueue::TestLEDAnimationsQueue() auto segment_1_desc = YAML::Load("{channel: 1, led_range: 0-10}"); auto segment_2_desc = YAML::Load("{channel: 2, led_range: 0-10}"); segments_.emplace( - "segment_1", std::make_shared(segment_1_desc, 50.0)); + "segment_1", std::make_shared(segment_1_desc, 50.0)); segments_.emplace( - "segment_2", std::make_shared(segment_2_desc, 50.0)); + "segment_2", std::make_shared(segment_2_desc, 50.0)); - led_anim_queue_ = std::make_shared(5); + led_anim_queue_ = std::make_shared(5); } -panther_lights::LEDAnimation TestLEDAnimationsQueue::CreateLEDAnimation( +husarion_ugv_lights::LEDAnimation TestLEDAnimationsQueue::CreateLEDAnimation( const std::string & name, const std::uint8_t priority, const rclcpp::Time & init_time) { - panther_lights::AnimationDescription anim_desc; + husarion_ugv_lights::AnimationDescription anim_desc; anim_desc.segments = {"segment_1", "segment_2"}; - anim_desc.type = "panther_lights::ImageAnimation"; + anim_desc.type = "husarion_ugv_lights::ImageAnimation"; anim_desc.animation = - YAML::Load("{image: $(find panther_lights)/animations/triangle01_red.png, duration: 2.0}"); + YAML::Load("{image: $(find husarion_ugv_lights)/test/files/animation.png, duration: 2.0}"); - panther_lights::LEDAnimationDescription led_anim_desc; + husarion_ugv_lights::LEDAnimationDescription led_anim_desc; led_anim_desc.id = 0; led_anim_desc.name = name; led_anim_desc.priority = priority; led_anim_desc.timeout = 10.0; led_anim_desc.animations = {anim_desc}; - return panther_lights::LEDAnimation(led_anim_desc, segments_, init_time); + return husarion_ugv_lights::LEDAnimation(led_anim_desc, segments_, init_time); } TEST_F(TestLEDAnimationsQueue, Put) { - auto led_anim = std::make_shared(CreateLEDAnimation("TEST", 1)); + auto led_anim = + std::make_shared(CreateLEDAnimation("TEST", 1)); led_anim_queue_->Put(led_anim, rclcpp::Time(0)); EXPECT_FALSE(led_anim_queue_->Empty()); @@ -84,7 +85,8 @@ TEST_F(TestLEDAnimationsQueue, Put) TEST_F(TestLEDAnimationsQueue, PutQueueOverloaded) { - auto led_anim = std::make_shared(CreateLEDAnimation("TEST", 1)); + auto led_anim = + std::make_shared(CreateLEDAnimation("TEST", 1)); for (std::size_t i = 0; i < max_queue_size_; i++) { led_anim_queue_->Put(led_anim, rclcpp::Time(0)); } @@ -95,11 +97,11 @@ TEST_F(TestLEDAnimationsQueue, PutQueueOverloaded) TEST_F(TestLEDAnimationsQueue, PutClearWhenPriorityEqualOne) { auto led_anim_pr_1 = - std::make_shared(CreateLEDAnimation("TEST", 1)); + std::make_shared(CreateLEDAnimation("TEST", 1)); auto led_anim_pr_2 = - std::make_shared(CreateLEDAnimation("TEST", 2)); + std::make_shared(CreateLEDAnimation("TEST", 2)); auto led_anim_pr_3 = - std::make_shared(CreateLEDAnimation("TEST", 3)); + std::make_shared(CreateLEDAnimation("TEST", 3)); led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); @@ -113,9 +115,9 @@ TEST_F(TestLEDAnimationsQueue, PutClearWhenPriorityEqualOne) TEST_F(TestLEDAnimationsQueue, PutSortByPriority) { auto led_anim_pr_2 = - std::make_shared(CreateLEDAnimation("TEST", 2)); + std::make_shared(CreateLEDAnimation("TEST", 2)); auto led_anim_pr_3 = - std::make_shared(CreateLEDAnimation("TEST", 3)); + std::make_shared(CreateLEDAnimation("TEST", 3)); led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); @@ -130,14 +132,14 @@ TEST_F(TestLEDAnimationsQueue, PutSortByPriority) TEST_F(TestLEDAnimationsQueue, PutSortByTime) { - auto led_anim_t0 = - std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); - auto led_anim_t1 = - std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(1))); - auto led_anim_t2 = - std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(2))); - auto led_anim_t3 = - std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(3))); + auto led_anim_t0 = std::make_shared( + CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); + auto led_anim_t1 = std::make_shared( + CreateLEDAnimation("TEST", 1, rclcpp::Time(1))); + auto led_anim_t2 = std::make_shared( + CreateLEDAnimation("TEST", 1, rclcpp::Time(2))); + auto led_anim_t3 = std::make_shared( + CreateLEDAnimation("TEST", 1, rclcpp::Time(3))); led_anim_queue_->Put(led_anim_t3, rclcpp::Time(4)); led_anim_queue_->Put(led_anim_t1, rclcpp::Time(4)); @@ -158,11 +160,11 @@ TEST_F(TestLEDAnimationsQueue, GetQueueEmpty) TEST_F(TestLEDAnimationsQueue, Clear) { auto led_anim_pr_1 = - std::make_shared(CreateLEDAnimation("TEST1", 1)); + std::make_shared(CreateLEDAnimation("TEST1", 1)); auto led_anim_pr_2 = - std::make_shared(CreateLEDAnimation("TEST2", 2)); + std::make_shared(CreateLEDAnimation("TEST2", 2)); auto led_anim_pr_3 = - std::make_shared(CreateLEDAnimation("TEST3", 3)); + std::make_shared(CreateLEDAnimation("TEST3", 3)); led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); @@ -184,8 +186,8 @@ TEST_F(TestLEDAnimationsQueue, Clear) TEST_F(TestLEDAnimationsQueue, ValidateAnimationTimedOut) { - auto led_anim = - std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); + auto led_anim = std::make_shared( + CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); led_anim_queue_->Put(led_anim, rclcpp::Time(0)); led_anim_queue_->Validate(rclcpp::Time(0)); @@ -200,11 +202,11 @@ TEST_F(TestLEDAnimationsQueue, ValidateAnimationTimedOut) TEST_F(TestLEDAnimationsQueue, GetFirstAnimationPriority) { auto led_anim_pr_1 = - std::make_shared(CreateLEDAnimation("TEST1", 1)); + std::make_shared(CreateLEDAnimation("TEST1", 1)); auto led_anim_pr_2 = - std::make_shared(CreateLEDAnimation("TEST2", 2)); + std::make_shared(CreateLEDAnimation("TEST2", 2)); auto led_anim_pr_3 = - std::make_shared(CreateLEDAnimation("TEST3", 3)); + std::make_shared(CreateLEDAnimation("TEST3", 3)); led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); @@ -223,11 +225,11 @@ TEST_F(TestLEDAnimationsQueue, GetFirstAnimationPriority) TEST_F(TestLEDAnimationsQueue, Remove) { auto led_anim_pr_1 = - std::make_shared(CreateLEDAnimation("TEST1", 1)); + std::make_shared(CreateLEDAnimation("TEST1", 1)); auto led_anim_pr_2 = - std::make_shared(CreateLEDAnimation("TEST2", 2)); + std::make_shared(CreateLEDAnimation("TEST2", 2)); auto led_anim_pr_3 = - std::make_shared(CreateLEDAnimation("TEST3", 3)); + std::make_shared(CreateLEDAnimation("TEST3", 3)); led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); diff --git a/panther_lights/test/unit/led_components/test_led_panel.cpp b/husarion_ugv_lights/test/unit/led_components/test_led_panel.cpp similarity index 93% rename from panther_lights/test/unit/led_components/test_led_panel.cpp rename to husarion_ugv_lights/test/unit/led_components/test_led_panel.cpp index 3b3c1aaf..7e0c90c3 100644 --- a/panther_lights/test/unit/led_components/test_led_panel.cpp +++ b/husarion_ugv_lights/test/unit/led_components/test_led_panel.cpp @@ -19,18 +19,18 @@ #include "gtest/gtest.h" -#include "panther_lights/led_components/led_panel.hpp" +#include "husarion_ugv_lights/led_components/led_panel.hpp" class TestLEDPanel : public testing::Test { public: - TestLEDPanel() { led_panel_ = std::make_unique(num_led_); } + TestLEDPanel() { led_panel_ = std::make_unique(num_led_); } ~TestLEDPanel() {} protected: void UpdateAndTestFrame( const std::size_t iterator_first, const std::vector & test_frame); - std::unique_ptr led_panel_; + std::unique_ptr led_panel_; const std::size_t num_led_ = 46; const std::size_t frame_size_ = num_led_ * 4; @@ -60,7 +60,7 @@ void TestLEDPanel::UpdateAndTestFrame( TEST(TestLEDPanelInitialization, FrameSize) { const std::size_t num_led = 22; - auto led_panel = panther_lights::LEDPanel(num_led); + auto led_panel = husarion_ugv_lights::LEDPanel(num_led); EXPECT_EQ(num_led * 4, led_panel.GetFrame().size()); } diff --git a/panther_lights/test/unit/led_components/test_led_segment.cpp b/husarion_ugv_lights/test/unit/led_components/test_led_segment.cpp similarity index 58% rename from panther_lights/test/unit/led_components/test_led_segment.cpp rename to husarion_ugv_lights/test/unit/led_components/test_led_segment.cpp index 9e20ff0d..9423f2fd 100644 --- a/panther_lights/test/unit/led_components/test_led_segment.cpp +++ b/husarion_ugv_lights/test/unit/led_components/test_led_segment.cpp @@ -21,10 +21,10 @@ #include "gtest/gtest.h" #include "yaml-cpp/yaml.h" -#include "panther_lights/led_components/led_segment.hpp" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_lights/led_components/led_segment.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" -class LEDSegmentWrapper : public panther_lights::LEDSegment +class LEDSegmentWrapper : public husarion_ugv_lights::LEDSegment { public: LEDSegmentWrapper(const YAML::Node & segment_description, const float controller_frequency) @@ -32,8 +32,8 @@ class LEDSegmentWrapper : public panther_lights::LEDSegment { } - std::shared_ptr GetAnimation() const { return animation_; } - std::shared_ptr GetDefaultAnimation() const + std::shared_ptr GetAnimation() const { return animation_; } + std::shared_ptr GetDefaultAnimation() const { return default_animation_; } @@ -67,121 +67,128 @@ YAML::Node CreateSegmentDescription(const std::string & led_range, const std::st TEST(TestLEDSegmentInitialization, DescriptionMissingRequiredKey) { auto segment_desc = YAML::Load(""); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); }, "Missing 'channel' in description")); segment_desc = YAML::Load("channel: 0"); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); }, "Missing 'led_range' in description")); } TEST(TestLEDSegmentInitialization, InvalidChannelExpression) { auto segment_desc = CreateSegmentDescription("0-10", "s1"); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); }, "Failed to convert 'channel' key")); segment_desc["channel"] = "-1"; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); }, "Failed to convert 'channel' key")); } TEST(TestLEDSegmentInitialization, InvalidLedRangeExpression) { auto segment_desc = CreateSegmentDescription("010", "1"); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); }, "No '-' character found in the led_range expression")); segment_desc["led_range"] = "s0-10"; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); }, "Error converting string to integer")); segment_desc["led_range"] = "0-p10"; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [segment_desc]() { husarion_ugv_lights::LEDSegment(segment_desc, 10.0); }, "Error converting string to integer")); } TEST(TestLEDSegmentInitialization, ValidDescription) { const auto segment_desc = CreateSegmentDescription("0-10", "1"); - EXPECT_NO_THROW(panther_lights::LEDSegment(segment_desc, 10.0)); + EXPECT_NO_THROW(husarion_ugv_lights::LEDSegment(segment_desc, 10.0)); } TEST(TestLEDSegmentInitialization, FirstLedPosition) { auto segment_desc = CreateSegmentDescription("0-10", "1"); - std::shared_ptr led_segment; + std::shared_ptr led_segment; - ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + ASSERT_NO_THROW( + led_segment = std::make_shared(segment_desc, 10.0)); EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition()); segment_desc["led_range"] = "5-11"; led_segment.reset(); - ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + ASSERT_NO_THROW( + led_segment = std::make_shared(segment_desc, 10.0)); EXPECT_EQ(std::size_t(5 * 4), led_segment->GetFirstLEDPosition()); segment_desc["led_range"] = "10-10"; led_segment.reset(); - ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + ASSERT_NO_THROW( + led_segment = std::make_shared(segment_desc, 10.0)); EXPECT_EQ(std::size_t(10 * 4), led_segment->GetFirstLEDPosition()); segment_desc["led_range"] = "13-5"; led_segment.reset(); - ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + ASSERT_NO_THROW( + led_segment = std::make_shared(segment_desc, 10.0)); EXPECT_EQ(std::size_t(5 * 4), led_segment->GetFirstLEDPosition()); segment_desc["led_range"] = "17-0"; led_segment.reset(); - ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + ASSERT_NO_THROW( + led_segment = std::make_shared(segment_desc, 10.0)); EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition()); } TEST_F(TestLEDSegment, GetAnimationFrameNoAnimation) { - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { led_segment_->GetAnimationFrame(); }, "Segment animation not defined")); } TEST_F(TestLEDSegment, GetAnimationProgressNoAnimation) { - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { led_segment_->GetAnimationProgress(); }, "Segment animation not defined")); } TEST_F(TestLEDSegment, ResetAnimationNoAnimation) { - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { led_segment_->ResetAnimation(); }, "Segment animation not defined")); } TEST_F(TestLEDSegment, GetAnimationBrightnessNoAnimation) { - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { led_segment_->GetAnimationBrightness(); }, "Segment animation not defined")); } TEST_F(TestLEDSegment, SetAnimationInvalidType) { const YAML::Node animation_desc; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { - led_segment_->SetAnimation("panther_lights::WrongAnimationType}", animation_desc, false); + led_segment_->SetAnimation("husarion_ugv_lights::WrongAnimationType}", animation_desc, false); }, "The plugin failed to load. Error: ")); } TEST_F(TestLEDSegment, SetAnimationFailAnimationInitialization) { - const auto animation_desc = YAML::Load("{type: panther_lights::ImageAnimation}"); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { led_segment_->SetAnimation("panther_lights::ImageAnimation", animation_desc, false); }, + const auto animation_desc = YAML::Load("{type: husarion_ugv_lights::ImageAnimation}"); + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { + led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", animation_desc, false); + }, "Failed to initialize animation: ")); } @@ -189,27 +196,29 @@ TEST_F(TestLEDSegment, SetAnimation) { // test each known animtion type const auto image_anim_desc = YAML::Load( - "{image: $(find panther_lights)/animations/triangle01_red.png, " + "{image: $(find husarion_ugv_lights)/test/files/animation.png, " "duration: 2}"); const auto charging_anim_desc = YAML::Load("{duration: 2}"); EXPECT_NO_THROW( - led_segment_->SetAnimation("panther_lights::ImageAnimation", image_anim_desc, false)); + led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", image_anim_desc, false)); EXPECT_NO_THROW(led_segment_->SetAnimation( - "panther_lights::ChargingAnimation", charging_anim_desc, false, "0.5")); + "husarion_ugv_lights::ChargingAnimation", charging_anim_desc, false, "0.5")); } TEST_F(TestLEDSegment, SetAnimationRepeating) { const auto anim_desc = YAML::Load( - "{image: $(find panther_lights)/animations/triangle01_red.png, " + "{image: $(find husarion_ugv_lights)/test/files/animation.png, " "duration: 2}"); - ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + ASSERT_NO_THROW( + led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false)); EXPECT_TRUE(led_segment_->GetDefaultAnimation().get() == nullptr); - ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, true)); + ASSERT_NO_THROW( + led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, true)); EXPECT_TRUE(led_segment_->GetDefaultAnimation().get() != nullptr); EXPECT_TRUE(led_segment_->IsAnimationFinished()); @@ -217,16 +226,17 @@ TEST_F(TestLEDSegment, SetAnimationRepeating) TEST_F(TestLEDSegment, UpdateAnimationAnimationNotSet) { - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { led_segment_->UpdateAnimation(); }, "Segment animation not defined")); } TEST_F(TestLEDSegment, UpdateAnimation) { const auto anim_desc = YAML::Load( - "{image: $(find panther_lights)/animations/triangle01_red.png, " + "{image: $(find husarion_ugv_lights)/test/files/animation.png, " "duration: 2}"); - ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + ASSERT_NO_THROW( + led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false)); EXPECT_NO_THROW(led_segment_->UpdateAnimation()); EXPECT_EQ(segment_led_num_ * 4, led_segment_->GetAnimationFrame().size()); } @@ -240,9 +250,10 @@ int main(int argc, char ** argv) TEST_F(TestLEDSegment, ResetDefaultAnimationWhenNewArrive) { const auto anim_desc = YAML::Load( - "{image: $(find panther_lights)/animations/triangle01_red.png, " + "{image: $(find husarion_ugv_lights)/test/files/animation.png, " "duration: 2}"); - ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, true)); + ASSERT_NO_THROW( + led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, true)); auto default_anim = led_segment_->GetDefaultAnimation(); while (!default_anim->IsFinished()) { @@ -250,7 +261,8 @@ TEST_F(TestLEDSegment, ResetDefaultAnimationWhenNewArrive) } // add new animation, and check if default animation was reset - ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + ASSERT_NO_THROW( + led_segment_->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false)); EXPECT_FALSE(default_anim->IsFinished()); } diff --git a/panther_lights/test/unit/led_components/test_segment_converter.cpp b/husarion_ugv_lights/test/unit/led_components/test_segment_converter.cpp similarity index 69% rename from panther_lights/test/unit/led_components/test_segment_converter.cpp rename to husarion_ugv_lights/test/unit/led_components/test_segment_converter.cpp index ef1a7747..53928704 100644 --- a/panther_lights/test/unit/led_components/test_segment_converter.cpp +++ b/husarion_ugv_lights/test/unit/led_components/test_segment_converter.cpp @@ -22,20 +22,20 @@ #include "gtest/gtest.h" #include "yaml-cpp/yaml.h" -#include "panther_lights/led_components/led_panel.hpp" -#include "panther_lights/led_components/led_segment.hpp" -#include "panther_lights/led_components/segment_converter.hpp" +#include "husarion_ugv_lights/led_components/led_panel.hpp" +#include "husarion_ugv_lights/led_components/led_segment.hpp" +#include "husarion_ugv_lights/led_components/segment_converter.hpp" class TestSegmentConverter : public testing::Test { public: TestSegmentConverter() { - segment_converter_ = std::make_unique(); + segment_converter_ = std::make_unique(); // create 2 basic panels with different number of leds - led_panels_.insert({1, std::make_unique(panel_1_num_led_)}); - led_panels_.insert({2, std::make_unique(panel_2_num_led_)}); + led_panels_.insert({1, std::make_unique(panel_1_num_led_)}); + led_panels_.insert({2, std::make_unique(panel_2_num_led_)}); } ~TestSegmentConverter() {} @@ -47,9 +47,9 @@ class TestSegmentConverter : public testing::Test std::size_t panel_1_num_led_ = 20; std::size_t panel_2_num_led_ = 30; - std::unique_ptr segment_converter_; - std::unordered_map> segments_; - std::unordered_map> led_panels_; + std::unique_ptr segment_converter_; + std::unordered_map> segments_; + std::unordered_map> led_panels_; }; YAML::Node TestSegmentConverter::CreateSegmentDescription( @@ -63,19 +63,17 @@ YAML::Node TestSegmentConverter::CreateSegmentDescription( YAML::Node TestSegmentConverter::CreateImageAnimationDescription() { - return YAML::Load( - "{image: $(find panther_lights)/animations/triangle01_red.png, " - "duration: 2}"); + return YAML::Load("{image: $(find husarion_ugv_lights)/test/files/animation.png, duration: 2}"); } TEST_F(TestSegmentConverter, ConvertInvalidChannel) { segments_.emplace( "name", - std::make_shared(CreateSegmentDescription(0, 10, 123), 50.0)); + std::make_shared(CreateSegmentDescription(0, 10, 123), 50.0)); const auto anim_desc = CreateImageAnimationDescription(); ASSERT_NO_THROW( - segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + segments_.at("name")->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false)); EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::out_of_range); } @@ -83,11 +81,11 @@ TEST_F(TestSegmentConverter, ConvertInvalidChannel) TEST_F(TestSegmentConverter, ConvertInvalidLedRange) { segments_.emplace( - "name", std::make_shared( + "name", std::make_shared( CreateSegmentDescription(panel_1_num_led_, panel_1_num_led_ + 1, 1), 50.0)); const auto anim_desc = CreateImageAnimationDescription(); ASSERT_NO_THROW( - segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + segments_.at("name")->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false)); EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::runtime_error); } @@ -95,17 +93,17 @@ TEST_F(TestSegmentConverter, ConvertInvalidLedRange) TEST_F(TestSegmentConverter, ConvertSingleSegmentForEachPanel) { segments_.emplace( - "name_1", std::make_shared( + "name_1", std::make_shared( CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0)); segments_.emplace( - "name_2", std::make_shared( + "name_2", std::make_shared( CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0)); const auto anim_desc = CreateImageAnimationDescription(); for (auto & segment : segments_) { ASSERT_NO_THROW( - segment.second->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + segment.second->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false)); ASSERT_NO_THROW(segment.second->UpdateAnimation()); } @@ -115,28 +113,28 @@ TEST_F(TestSegmentConverter, ConvertSingleSegmentForEachPanel) TEST_F(TestSegmentConverter, ConvertMultipleSegments) { segments_.emplace( - "name_1", std::make_shared( + "name_1", std::make_shared( CreateSegmentDescription(0, std::size_t(panel_1_num_led_ / 2) - 1, 1), 50.0)); segments_.emplace( "name_2", - std::make_shared( + std::make_shared( CreateSegmentDescription(std::size_t(panel_1_num_led_ / 2), panel_1_num_led_ - 1, 1), 50.0)); segments_.emplace( - "name_3", std::make_shared( + "name_3", std::make_shared( CreateSegmentDescription(0, (panel_2_num_led_ / 4) - 1, 2), 50.0)); segments_.emplace( "name_4", - std::make_shared( + std::make_shared( CreateSegmentDescription((panel_2_num_led_ / 4), (panel_2_num_led_ / 2) - 1, 2), 50.0)); segments_.emplace( - "name_5", std::make_shared( + "name_5", std::make_shared( CreateSegmentDescription((panel_2_num_led_ / 2), panel_2_num_led_ - 1, 2), 50.0)); const auto anim_desc = CreateImageAnimationDescription(); for (auto & segment : segments_) { ASSERT_NO_THROW( - segment.second->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + segment.second->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false)); ASSERT_NO_THROW(segment.second->UpdateAnimation()); } @@ -152,11 +150,11 @@ TEST_F(TestSegmentConverter, ConvertBrightnessOverride) anim_desc["brightness"] = float_brightness; segments_.emplace( - "name", std::make_shared( + "name", std::make_shared( CreateSegmentDescription(0, panel_1_num_led_ - 1, channel), 50.0)); ASSERT_NO_THROW( - segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + segments_.at("name")->SetAnimation("husarion_ugv_lights::ImageAnimation", anim_desc, false)); segment_converter_->Convert(segments_, led_panels_); ASSERT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); @@ -171,10 +169,10 @@ TEST_F(TestSegmentConverter, ConvertBrightnessOverride) TEST_F(TestSegmentConverter, ConvertNoThrowIfAnimationNotSet) { segments_.emplace( - "name_1", std::make_shared( + "name_1", std::make_shared( CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0)); segments_.emplace( - "name_2", std::make_shared( + "name_2", std::make_shared( CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0)); EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); diff --git a/panther_lights/test/unit/test_apa102.cpp b/husarion_ugv_lights/test/unit/test_apa102.cpp similarity index 97% rename from panther_lights/test/unit/test_apa102.cpp rename to husarion_ugv_lights/test/unit/test_apa102.cpp index 2f5bbd2f..2d43684c 100644 --- a/panther_lights/test/unit/test_apa102.cpp +++ b/husarion_ugv_lights/test/unit/test_apa102.cpp @@ -15,13 +15,13 @@ #include #include -#include "panther_lights/apa102.hpp" +#include "husarion_ugv_lights/apa102.hpp" static constexpr char kMockDeviceName[] = "/dev/mocked_device"; static constexpr int kStartFrame = 0x00; static constexpr int kEndFrame = 0xFF; -class MockSPIDevice : public panther_lights::SPIDeviceInterface +class MockSPIDevice : public husarion_ugv_lights::SPIDeviceInterface { public: MOCK_METHOD(int, Open, (const std::string & device), (override)); @@ -32,7 +32,7 @@ class MockSPIDevice : public panther_lights::SPIDeviceInterface using NiceMock = testing::NiceMock; }; -class APA102Wrapper : public panther_lights::APA102 +class APA102Wrapper : public husarion_ugv_lights::APA102 { public: APA102Wrapper(std::shared_ptr spi_device, const std::string & device_name) diff --git a/panther_lights/test/unit/test_lights_controller_node.cpp b/husarion_ugv_lights/test/unit/test_lights_controller_node.cpp similarity index 79% rename from panther_lights/test/unit/test_lights_controller_node.cpp rename to husarion_ugv_lights/test/unit/test_lights_controller_node.cpp index 7a27f3a4..07546ac4 100644 --- a/panther_lights/test/unit/test_lights_controller_node.cpp +++ b/husarion_ugv_lights/test/unit/test_lights_controller_node.cpp @@ -26,10 +26,10 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_lights/lights_controller_node.hpp" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_lights/lights_controller_node.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" -class ControllerNodeWrapper : public panther_lights::LightsControllerNode +class ControllerNodeWrapper : public husarion_ugv_lights::LightsControllerNode { public: ControllerNodeWrapper(const rclcpp::NodeOptions & options) : LightsControllerNode(options) {} @@ -55,9 +55,12 @@ class ControllerNodeWrapper : public panther_lights::LightsControllerNode return LightsControllerNode::AddAnimationToQueue(animation_id, repeating, param); } - std::shared_ptr GetQueue() { return this->animations_queue_; } + std::shared_ptr GetQueue() + { + return this->animations_queue_; + } - std::shared_ptr GetCurrentAnimation() + std::shared_ptr GetCurrentAnimation() { return this->current_animation_; } @@ -77,18 +80,18 @@ class TestLightsControllerNode : public testing::Test static constexpr char kTestSegmentName[] = "test"; static constexpr char kTestSegmentLedRange[] = "0-9"; - std::filesystem::path led_config_file_; + std::filesystem::path animations_config_path_; std::shared_ptr lights_controller_node_; }; TestLightsControllerNode::TestLightsControllerNode() { - led_config_file_ = testing::TempDir() + "/led_config.yaml"; + animations_config_path_ = testing::TempDir() + "/animations.yaml"; - CreateLEDConfig(led_config_file_); + CreateLEDConfig(animations_config_path_); std::vector params; - params.push_back(rclcpp::Parameter("led_config_file", led_config_file_)); + params.push_back(rclcpp::Parameter("animations_config_path", animations_config_path_)); rclcpp::NodeOptions options; options.parameter_overrides(params); @@ -96,7 +99,10 @@ TestLightsControllerNode::TestLightsControllerNode() lights_controller_node_ = std::make_shared(options); } -TestLightsControllerNode::~TestLightsControllerNode() { std::filesystem::remove(led_config_file_); } +TestLightsControllerNode::~TestLightsControllerNode() +{ + std::filesystem::remove(animations_config_path_); +} void TestLightsControllerNode::CreateLEDConfig(const std::filesystem::path file_path) { @@ -113,11 +119,11 @@ void TestLightsControllerNode::CreateLEDConfig(const std::filesystem::path file_ segments_map["test"] = std::vector(1, kTestSegmentName); YAML::Node animation; - animation["image"] = "$(find panther_lights)/animations/triangle01_red.png"; + animation["image"] = "$(find husarion_ugv_lights)/test/files/strip01_red.png"; animation["duration"] = 2; YAML::Node animation_desc; - animation_desc["type"] = "panther_lights::ImageAnimation"; + animation_desc["type"] = "husarion_ugv_lights::ImageAnimation"; animation_desc["segments"] = "test"; animation_desc["animation"] = animation; @@ -133,14 +139,14 @@ void TestLightsControllerNode::CreateLEDConfig(const std::filesystem::path file_ led_animations.push_back(led_animation_0); led_animations.push_back(led_animation_1); - YAML::Node led_config; - led_config["panels"] = std::vector(1, panel); - led_config["segments"] = std::vector(1, segment); - led_config["segments_map"] = segments_map; - led_config["led_animations"] = led_animations; + YAML::Node animations_config; + animations_config["panels"] = std::vector(1, panel); + animations_config["segments"] = std::vector(1, segment); + animations_config["segments_map"] = segments_map; + animations_config["led_animations"] = led_animations; YAML::Emitter out; - out << led_config; + out << animations_config; std::ofstream fout(file_path); if (fout.is_open()) { @@ -168,7 +174,7 @@ TEST_F(TestLightsControllerNode, InitializeLEDPanelsThrowRepeatingChannel) YAML::Node panels_desc; panels_desc["panels"] = panels; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { lights_controller_node_->InitializeLEDPanels(panels_desc["panels"]); }, "Multiple panels with channel nr")); } @@ -192,7 +198,7 @@ TEST_F(TestLightsControllerNode, InitializeLEDSegmentsThrowRepeatingName) YAML::Node segments_desc; segments_desc["segments"] = segments; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { lights_controller_node_->InitializeLEDSegments(segments_desc["segments"], 50.0); }, "Multiple segments with given name found")); } @@ -203,13 +209,13 @@ TEST_F(TestLightsControllerNode, LoadAnimationInvalidPriority) led_animation_desc["id"] = 11; led_animation_desc["priority"] = 0; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { lights_controller_node_->LoadAnimation(led_animation_desc); }, "Invalid LED animation priority")); led_animation_desc["priority"] = 4; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { lights_controller_node_->LoadAnimation(led_animation_desc); }, "Invalid LED animation priority")); } @@ -222,14 +228,14 @@ TEST_F(TestLightsControllerNode, LoadAnimationThrowRepeatingID) ASSERT_NO_THROW(lights_controller_node_->LoadAnimation(led_animation_desc)); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { lights_controller_node_->LoadAnimation(led_animation_desc); }, "Animation with given ID already exists")); } TEST_F(TestLightsControllerNode, AddAnimationToQueueThrowBadAnimationID) { - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { lights_controller_node_->AddAnimationToQueue(99, false); }, "No animation with ID:")); } diff --git a/panther_lights/test/unit/test_lights_driver_node.cpp b/husarion_ugv_lights/test/unit/test_lights_driver_node.cpp similarity index 81% rename from panther_lights/test/unit/test_lights_driver_node.cpp rename to husarion_ugv_lights/test/unit/test_lights_driver_node.cpp index 9a20b0f5..91fcc0c1 100644 --- a/panther_lights/test/unit/test_lights_driver_node.cpp +++ b/husarion_ugv_lights/test/unit/test_lights_driver_node.cpp @@ -25,15 +25,15 @@ #include "panther_msgs/srv/set_led_brightness.hpp" -#include "panther_lights/apa102.hpp" -#include "panther_lights/lights_driver_node.hpp" -#include "panther_utils/test/ros_test_utils.hpp" +#include "husarion_ugv_lights/apa102.hpp" +#include "husarion_ugv_lights/lights_driver_node.hpp" +#include "husarion_ugv_utils/test/ros_test_utils.hpp" using ImageMsg = sensor_msgs::msg::Image; using SetBoolSrv = std_srvs::srv::SetBool; using SetLEDBrightnessSrv = panther_msgs::srv::SetLEDBrightness; -class MockAPA102 : public panther_lights::APA102Interface +class MockAPA102 : public husarion_ugv_lights::APA102Interface { public: MOCK_METHOD(void, SetGlobalBrightness, (const std::uint8_t brightness), (override)); @@ -44,7 +44,7 @@ class MockAPA102 : public panther_lights::APA102Interface }; // LightsDriverNode constructor implemented for testing purposes -panther_lights::LightsDriverNode::LightsDriverNode( +husarion_ugv_lights::LightsDriverNode::LightsDriverNode( APA102Interface::SharedPtr channel_1, APA102Interface::SharedPtr channel_2, const rclcpp::NodeOptions & options) : Node("lights_driver", options), @@ -55,11 +55,12 @@ panther_lights::LightsDriverNode::LightsDriverNode( channel_2_(channel_2), diagnostic_updater_(this) { - num_led_ = 46; + channel_1_num_led_ = 46; + channel_2_num_led_ = 46; frame_timeout_ = 0.1; }; -class DriverNodeWrapper : public panther_lights::LightsDriverNode +class DriverNodeWrapper : public husarion_ugv_lights::LightsDriverNode { public: DriverNodeWrapper( @@ -77,13 +78,16 @@ class DriverNodeWrapper : public panther_lights::LightsDriverNode } void FrameCB( - const ImageMsg::UniquePtr & msg, const panther_lights::APA102Interface::SharedPtr & panel, + const ImageMsg::UniquePtr & msg, const husarion_ugv_lights::APA102Interface::SharedPtr & panel, const rclcpp::Time & last_time, const std::string & panel_name) { return LightsDriverNode::FrameCB(msg, panel, last_time, panel_name); } - int GetNumLed() const { return num_led_; } + int GetNumLed(const std::string & panel_name) const + { + return panel_name == "channel_1" ? channel_1_num_led_ : channel_2_num_led_; + } double GetTimeout() const { return frame_timeout_; } bool GetLedControlGranted() const { return led_control_granted_; } bool GetLedControlPending() const { return led_control_pending_; } @@ -98,7 +102,7 @@ class TestLightsDriverNode : public testing::Test TestLightsDriverNode(); ~TestLightsDriverNode() {}; - ImageMsg::UniquePtr CreateValidImageMsg(); + ImageMsg::UniquePtr CreateValidImageMsg(const std::string & panel_name); std::shared_future> CreateSetBoolSrvFuture(bool request_data, bool response_success); @@ -115,7 +119,7 @@ TestLightsDriverNode::TestLightsDriverNode() lights_driver_node_ = std::make_unique(channel_1_, channel_2_); } -ImageMsg::UniquePtr TestLightsDriverNode::CreateValidImageMsg() +ImageMsg::UniquePtr TestLightsDriverNode::CreateValidImageMsg(const std::string & panel_name) { ImageMsg::UniquePtr msg(new ImageMsg); @@ -123,7 +127,7 @@ ImageMsg::UniquePtr TestLightsDriverNode::CreateValidImageMsg() msg->header.stamp = lights_driver_node_->now(); msg->header.frame_id = "image_frame"; msg->height = 1; - msg->width = lights_driver_node_->GetNumLed(); + msg->width = lights_driver_node_->GetNumLed(panel_name); msg->encoding = sensor_msgs::image_encodings::RGBA8; msg->is_bigendian = false; msg->step = msg->width * 4; @@ -158,11 +162,13 @@ TEST_F(TestLightsDriverNode, TestInitialization) TEST_F(TestLightsDriverNode, ClearLEDs) { - auto num_led = lights_driver_node_->GetNumLed(); - std::vector zero_frame(num_led * 4, 0); + auto num_led_1 = lights_driver_node_->GetNumLed("channel_1"); + auto num_led_2 = lights_driver_node_->GetNumLed("channel_2"); + std::vector zero_frame_1(num_led_1 * 4, 0); + std::vector zero_frame_2(num_led_2 * 4, 0); - EXPECT_CALL(*channel_1_, SetPanel(zero_frame)).Times(1); - EXPECT_CALL(*channel_2_, SetPanel(zero_frame)).Times(1); + EXPECT_CALL(*channel_1_, SetPanel(zero_frame_1)).Times(1); + EXPECT_CALL(*channel_2_, SetPanel(zero_frame_2)).Times(1); lights_driver_node_->ClearLEDs(); } @@ -203,7 +209,7 @@ TEST_F(TestLightsDriverNode, ToggleLEDControlCBDisabled) TEST_F(TestLightsDriverNode, FrameCBSuccessNoControl) { - auto msg = CreateValidImageMsg(); + auto msg = CreateValidImageMsg("channel_1"); EXPECT_CALL(*channel_1_, SetPanel(testing::_)).Times(0); @@ -212,7 +218,8 @@ TEST_F(TestLightsDriverNode, FrameCBSuccessNoControl) TEST_F(TestLightsDriverNode, FrameCBSuccess) { - auto msg = CreateValidImageMsg(); + auto msg_1 = CreateValidImageMsg("channel_1"); + auto msg_2 = CreateValidImageMsg("channel_2"); auto future = CreateSetBoolSrvFuture(true, true); lights_driver_node_->ToggleLEDControlCB(std::move(future)); @@ -220,13 +227,13 @@ TEST_F(TestLightsDriverNode, FrameCBSuccess) EXPECT_CALL(*channel_1_, SetPanel(testing::_)).Times(1); EXPECT_CALL(*channel_2_, SetPanel(testing::_)).Times(1); - lights_driver_node_->FrameCB(msg, channel_1_, msg->header.stamp, "channel_1"); - lights_driver_node_->FrameCB(msg, channel_2_, msg->header.stamp, "channel_2"); + lights_driver_node_->FrameCB(msg_1, channel_1_, msg_1->header.stamp, "channel_1"); + lights_driver_node_->FrameCB(msg_2, channel_2_, msg_2->header.stamp, "channel_2"); } TEST_F(TestLightsDriverNode, FrameCBTimeout) { - auto msg = CreateValidImageMsg(); + auto msg = CreateValidImageMsg("channel_1"); auto timeout = lights_driver_node_->GetTimeout(); // Set timestamp to exceed timeout @@ -242,7 +249,7 @@ TEST_F(TestLightsDriverNode, FrameCBTimeout) TEST_F(TestLightsDriverNode, FrameCBPast) { - auto msg = CreateValidImageMsg(); + auto msg = CreateValidImageMsg("channel_1"); // Set last_time to be younger than msg timestamp auto future = CreateSetBoolSrvFuture(true, true); @@ -255,7 +262,7 @@ TEST_F(TestLightsDriverNode, FrameCBPast) TEST_F(TestLightsDriverNode, FrameCBEncoding) { - auto msg = CreateValidImageMsg(); + auto msg = CreateValidImageMsg("channel_1"); // Set incorrect encoding msg->encoding = sensor_msgs::image_encodings::RGB8; @@ -270,7 +277,7 @@ TEST_F(TestLightsDriverNode, FrameCBEncoding) TEST_F(TestLightsDriverNode, FrameCBHeight) { - auto msg = CreateValidImageMsg(); + auto msg = CreateValidImageMsg("channel_1"); // Set incorrect height msg->height = 2; @@ -285,10 +292,10 @@ TEST_F(TestLightsDriverNode, FrameCBHeight) TEST_F(TestLightsDriverNode, FrameCBWidth) { - auto msg = CreateValidImageMsg(); + auto msg = CreateValidImageMsg("channel_1"); // Set incorrect width - msg->width = lights_driver_node_->GetNumLed() + 1; + msg->width = lights_driver_node_->GetNumLed("channel_1") + 1; auto future = CreateSetBoolSrvFuture(true, true); lights_driver_node_->ToggleLEDControlCB(std::move(future)); diff --git a/panther_localization/CHANGELOG.rst b/husarion_ugv_localization/CHANGELOG.rst similarity index 100% rename from panther_localization/CHANGELOG.rst rename to husarion_ugv_localization/CHANGELOG.rst diff --git a/panther_controller/CMakeLists.txt b/husarion_ugv_localization/CMakeLists.txt similarity index 82% rename from panther_controller/CMakeLists.txt rename to husarion_ugv_localization/CMakeLists.txt index c6489681..2ce65ec9 100644 --- a/panther_controller/CMakeLists.txt +++ b/husarion_ugv_localization/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(panther_controller) +project(husarion_ugv_localization) find_package(ament_cmake REQUIRED) diff --git a/panther_localization/README.md b/husarion_ugv_localization/README.md similarity index 90% rename from panther_localization/README.md rename to husarion_ugv_localization/README.md index 416af897..54eacef5 100644 --- a/panther_localization/README.md +++ b/husarion_ugv_localization/README.md @@ -1,4 +1,4 @@ -# panther_localization +# husarion_ugv_localization The package is responsible for activating mods responsible for fusion of data related to the robot's location. @@ -13,6 +13,6 @@ This package contains: - [`enu_localization.yaml`](./config/enu_localization.yaml): configures data fusion for `ekf_filter` and `navsat_transform` nodes, using **wheel encoders** and **IMU**. Orientation follows East-North-Up (ENU) coordinates. - [`enu_localization_with_gps.yaml`](./config/enu_localization_with_gps.yaml): configures data fusion for `ekf_filter` and `navsat_transform` nodes, using **wheel encoders**, **IMU**, and **GPS**. Orientation follows East-North-Up (ENU) coordinates. -- [`nmea_navsat_params.yaml`](./config/nmea_navsat_params.yaml): contains parameters for NMEA NavSat driver node. +- [`nmea_navsat.yaml`](./config/nmea_navsat.yaml): contains parameters for NMEA NavSat driver node. - [`relative_localization.yaml`](./config/relative_localization.yaml): configures data fusion for `ekf_filter` and `navsat_transform` nodes, using **wheel encoders**, **IMU**. The initial orientation is always 0 in relative mode. - [`relative_localization_with_gps.yaml`](./config/relative_localization_with_gps.yaml): configures data fusion for `ekf_filter` and `navsat_transform` nodes, using **wheel encoders**, **IMU**, and **GPS**. The initial orientation is always 0 in relative mode. diff --git a/panther_localization/config/enu_localization.yaml b/husarion_ugv_localization/config/enu_localization.yaml similarity index 99% rename from panther_localization/config/enu_localization.yaml rename to husarion_ugv_localization/config/enu_localization.yaml index afaee4e2..6b2668cc 100644 --- a/panther_localization/config/enu_localization.yaml +++ b/husarion_ugv_localization/config/enu_localization.yaml @@ -41,7 +41,7 @@ use_control: true control_timeout: 0.5 control_config: [true, true, false, false, false, true] - acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside panther_controller/config + acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config predict_to_current_time: true diff --git a/panther_localization/config/enu_localization_with_gps.yaml b/husarion_ugv_localization/config/enu_localization_with_gps.yaml similarity index 99% rename from panther_localization/config/enu_localization_with_gps.yaml rename to husarion_ugv_localization/config/enu_localization_with_gps.yaml index 96c60320..ef1fa6ff 100644 --- a/panther_localization/config/enu_localization_with_gps.yaml +++ b/husarion_ugv_localization/config/enu_localization_with_gps.yaml @@ -51,7 +51,7 @@ use_control: true control_timeout: 0.5 control_config: [true, true, false, false, false, true] - acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside panther_controller/config + acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config predict_to_current_time: true diff --git a/panther_localization/config/nmea_navsat_params.yaml b/husarion_ugv_localization/config/nmea_navsat.yaml similarity index 100% rename from panther_localization/config/nmea_navsat_params.yaml rename to husarion_ugv_localization/config/nmea_navsat.yaml diff --git a/panther_localization/config/relative_localization.yaml b/husarion_ugv_localization/config/relative_localization.yaml similarity index 99% rename from panther_localization/config/relative_localization.yaml rename to husarion_ugv_localization/config/relative_localization.yaml index 3e29fcdd..5f420735 100644 --- a/panther_localization/config/relative_localization.yaml +++ b/husarion_ugv_localization/config/relative_localization.yaml @@ -41,7 +41,7 @@ use_control: true control_timeout: 0.5 control_config: [true, true, false, false, false, true] - acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside panther_controller/config + acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config predict_to_current_time: true diff --git a/panther_localization/config/relative_localization_with_gps.yaml b/husarion_ugv_localization/config/relative_localization_with_gps.yaml similarity index 99% rename from panther_localization/config/relative_localization_with_gps.yaml rename to husarion_ugv_localization/config/relative_localization_with_gps.yaml index 0a98abcb..fa842675 100644 --- a/panther_localization/config/relative_localization_with_gps.yaml +++ b/husarion_ugv_localization/config/relative_localization_with_gps.yaml @@ -51,7 +51,7 @@ use_control: true control_timeout: 0.5 control_config: [true, true, false, false, false, true] - acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside panther_controller/config + acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config predict_to_current_time: true diff --git a/panther_localization/launch/localization.launch.py b/husarion_ugv_localization/launch/localization.launch.py similarity index 90% rename from panther_localization/launch/localization.launch.py rename to husarion_ugv_localization/launch/localization.launch.py index 2645a6b4..f03843e7 100644 --- a/panther_localization/launch/localization.launch.py +++ b/husarion_ugv_localization/launch/localization.launch.py @@ -64,6 +64,14 @@ def generate_launch_description(): description="Add namespace to all launched nodes.", ) + use_ekf = LaunchConfiguration("use_ekf") + declare_use_ekf_arg = DeclareLaunchArgument( + "use_ekf", + default_value="True", + description="Enable or disable EKF.", + choices=["True", "true", "False", "false"], + ) + use_sim = LaunchConfiguration("use_sim") declare_use_sim_arg = DeclareLaunchArgument( "use_sim", @@ -82,7 +90,7 @@ def generate_launch_description(): declare_localization_config_path_arg = DeclareLaunchArgument( "localization_config_path", default_value=PathJoinSubstitution( - [FindPackageShare("panther_localization"), "config", localization_config_filename] + ["/config", "husarion_ugv_localization", "config", localization_config_filename] ), description="Specify the path to the localization configuration file.", ) @@ -99,12 +107,13 @@ def generate_launch_description(): ("set_pose", "localization/set_pose"), ("toggle", "localization/toggle"), ], + condition=IfCondition(use_ekf), ) nmea_navsat_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_localization"), "launch", "nmea_navsat.launch.py"] + [FindPackageShare("husarion_ugv_localization"), "launch", "nmea_navsat.launch.py"] ) ), launch_arguments={"namespace": namespace}.items(), @@ -131,6 +140,7 @@ def generate_launch_description(): declare_localization_mode_arg, declare_localization_config_path_arg, # localization_config_path use fuse_gps and localization_mode declare_namespace_arg, + declare_use_ekf_arg, declare_use_sim_arg, SetParameter(name="use_sim_time", value=use_sim), ekf_filter_node, diff --git a/panther_localization/launch/nmea_navsat.launch.py b/husarion_ugv_localization/launch/nmea_navsat.launch.py similarity index 76% rename from panther_localization/launch/nmea_navsat.launch.py rename to husarion_ugv_localization/launch/nmea_navsat.launch.py index 585438f2..70fe996d 100644 --- a/panther_localization/launch/nmea_navsat.launch.py +++ b/husarion_ugv_localization/launch/nmea_navsat.launch.py @@ -26,18 +26,12 @@ def generate_launch_description(): - device_namespace = LaunchConfiguration("device_namespace") - declare_device_namespace_arg = DeclareLaunchArgument( - "device_namespace", - default_value="gps", - description="Namespace for the device, utilized in TF frames and preceding device topics. This aids in differentiating between multiple cameras on the same robot.", - ) - params_file = LaunchConfiguration("params_file") - declare_params_file_arg = DeclareLaunchArgument( - "params_file", + nmea_params_path = LaunchConfiguration("nmea_params_path") + declare_nmea_params_path_arg = DeclareLaunchArgument( + "nmea_params_path", default_value=PathJoinSubstitution( - [FindPackageShare("panther_localization"), "config", "nmea_navsat_params.yaml"] + [FindPackageShare("husarion_ugv_localization"), "config", "nmea_navsat.yaml"] ), description="Path to the parameter file for the nmea_socket_driver node.", ) @@ -49,6 +43,7 @@ def generate_launch_description(): description="Namespace to all launched nodes and use namespace as tf_prefix. This aids in differentiating between multiple robots with the same devices.", ) + device_namespace = "gps" nmea_driver_node = Node( package="nmea_navsat_driver", executable="nmea_socket_driver", @@ -59,7 +54,7 @@ def generate_launch_description(): "frame_id": device_namespace, "tf_prefix": namespace, }, - params_file, + nmea_params_path, ], remappings=[ ("fix", [device_namespace, "/fix"]), @@ -71,9 +66,8 @@ def generate_launch_description(): return LaunchDescription( [ - declare_params_file_arg, + declare_nmea_params_path_arg, declare_robot_namespace_arg, - declare_device_namespace_arg, nmea_driver_node, ] ) diff --git a/panther_localization/package.xml b/husarion_ugv_localization/package.xml similarity index 88% rename from panther_localization/package.xml rename to husarion_ugv_localization/package.xml index 10d1fe44..ed468ba2 100644 --- a/panther_localization/package.xml +++ b/husarion_ugv_localization/package.xml @@ -1,9 +1,9 @@ - panther_localization + husarion_ugv_localization 2.1.2 - robot localization configuration for Panther + robot localization configuration for Husarion UGV Husarion Apache License 2.0 diff --git a/panther_manager/CHANGELOG.rst b/husarion_ugv_manager/CHANGELOG.rst similarity index 100% rename from panther_manager/CHANGELOG.rst rename to husarion_ugv_manager/CHANGELOG.rst diff --git a/panther_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt similarity index 86% rename from panther_manager/CMakeLists.txt rename to husarion_ugv_manager/CMakeLists.txt index fbf7fc92..82f9b528 100644 --- a/panther_manager/CMakeLists.txt +++ b/husarion_ugv_manager/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(panther_manager) +project(husarion_ugv_manager) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -10,9 +10,10 @@ set(PACKAGE_DEPENDENCIES ament_index_cpp behaviortree_cpp behaviortree_ros2 + generate_parameter_library libssh panther_msgs - panther_utils + husarion_ugv_utils rclcpp rclcpp_action sensor_msgs @@ -67,11 +68,15 @@ ament_target_dependencies( safety_manager_node behaviortree_ros2 panther_msgs - panther_utils + husarion_ugv_utils rclcpp sensor_msgs std_msgs) -target_link_libraries(safety_manager_node ${plugin_libs}) + +generate_parameter_library(safety_manager_parameters + config/safety_manager_parameters.yaml) +target_link_libraries(safety_manager_node ${plugin_libs} + safety_manager_parameters) add_executable( lights_manager_node src/lights_manager_node_main.cpp @@ -80,11 +85,15 @@ ament_target_dependencies( lights_manager_node behaviortree_ros2 panther_msgs - panther_utils + husarion_ugv_utils rclcpp sensor_msgs std_msgs) -target_link_libraries(lights_manager_node ${plugin_libs}) + +generate_parameter_library(lights_manager_parameters + config/lights_manager_parameters.yaml) +target_link_libraries(lights_manager_node ${plugin_libs} + lights_manager_parameters) install(TARGETS ${plugin_libs} DESTINATION lib) @@ -167,8 +176,9 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_behavior_tree_utils PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_behavior_tree_utils - behaviortree_cpp behaviortree_ros2 panther_utils) + ament_target_dependencies( + ${PROJECT_NAME}_test_behavior_tree_utils behaviortree_cpp behaviortree_ros2 + husarion_ugv_utils) ament_add_gtest( ${PROJECT_NAME}_test_behavior_tree_manager @@ -178,7 +188,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_behavior_tree_manager - behaviortree_cpp panther_utils) + behaviortree_cpp husarion_ugv_utils) ament_add_gtest( ${PROJECT_NAME}_test_lights_manager_node test/test_lights_manager_node.cpp @@ -192,10 +202,12 @@ if(BUILD_TESTING) behaviortree_cpp behaviortree_ros2 panther_msgs - panther_utils + husarion_ugv_utils rclcpp sensor_msgs std_msgs) + target_link_libraries(${PROJECT_NAME}_test_lights_manager_node + lights_manager_parameters) ament_add_gtest( ${PROJECT_NAME}_test_lights_behavior_tree @@ -210,10 +222,12 @@ if(BUILD_TESTING) behaviortree_cpp behaviortree_ros2 panther_msgs - panther_utils + husarion_ugv_utils rclcpp sensor_msgs std_msgs) + target_link_libraries(${PROJECT_NAME}_test_lights_behavior_tree + lights_manager_parameters) ament_add_gtest( ${PROJECT_NAME}_test_safety_manager_node test/test_safety_manager_node.cpp @@ -227,10 +241,12 @@ if(BUILD_TESTING) behaviortree_cpp behaviortree_ros2 panther_msgs - panther_utils + husarion_ugv_utils rclcpp sensor_msgs std_msgs) + target_link_libraries(${PROJECT_NAME}_test_safety_manager_node + safety_manager_parameters) ament_add_gtest( ${PROJECT_NAME}_test_safety_behavior_tree @@ -245,11 +261,13 @@ if(BUILD_TESTING) behaviortree_cpp behaviortree_ros2 panther_msgs - panther_utils + husarion_ugv_utils rclcpp sensor_msgs std_msgs std_srvs) + target_link_libraries(${PROJECT_NAME}_test_safety_behavior_tree + safety_manager_parameters) endif() ament_package() diff --git a/panther_manager/CONFIGURATION.md b/husarion_ugv_manager/CONFIGURATION.md similarity index 96% rename from panther_manager/CONFIGURATION.md rename to husarion_ugv_manager/CONFIGURATION.md index 3317e43a..adc2337a 100644 --- a/panther_manager/CONFIGURATION.md +++ b/husarion_ugv_manager/CONFIGURATION.md @@ -1,11 +1,11 @@ -# panther_manager +# husarion_ugv_manager ## Shutdown Behavior For more information regarding shutdown behavior, refer to `ShutdownSingleHost` BT node in the [Actions](#actions) section. An example of a shutdown hosts YAML file can be found below. ``` yaml -# My shutdown_hosts_config.yaml +# My shutdown_hosts.yaml hosts: # Intel NUC, user computer - ip: 10.15.20.3 @@ -39,7 +39,7 @@ ssh-copy-id username@10.15.20.XX ## Faults Handle -After receiving a message on the `battery/battery_status` topic, the `panther_manager` node makes decisions regarding safety measures. For more information regarding the power supply status, please refer to the [BatteryState](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) message definition and [adc_battery.cpp](../panther_battery/src/battery/adc_battery.cpp) implementation. +After receiving a message on the `battery/battery_status` topic, the `husarion_ugv_manager` node makes decisions regarding safety measures. For more information regarding the power supply status, please refer to the [BatteryState](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) message definition and [adc_battery.cpp](../husarion_ugv_battery/src/battery/adc_battery.cpp) implementation. | Power Supply Health | Procedure | | ------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | @@ -204,7 +204,7 @@ To use your customized project, you can modify the `bt_project_file` ROS paramet ### Real-time Visualization -Groot2 also provides a real-time visualization tool that allows you to see and debug actively running trees. To use this tool with trees launched with the `panther_manager` package, you need to specify the port associated with the tree you want to visualize. The ports for each tree are listed below: +Groot2 also provides a real-time visualization tool that allows you to see and debug actively running trees. To use this tool with trees launched with the `husarion_ugv_manager` package, you need to specify the port associated with the tree you want to visualize. The ports for each tree are listed below: - Lights tree: `10.15.20.2:5555` - Safety tree: `10.15.20.2:6666` diff --git a/panther_manager/README.md b/husarion_ugv_manager/README.md similarity index 87% rename from panther_manager/README.md rename to husarion_ugv_manager/README.md index b4ea0936..f43f5657 100644 --- a/panther_manager/README.md +++ b/husarion_ugv_manager/README.md @@ -1,6 +1,6 @@ -# panther_manager +# husarion_ugv_manager -A package containing nodes responsible for high-level control of Husarion Panther robot. +A package containing nodes responsible for high-level control of Husarion UGV. ## Launch Files @@ -11,13 +11,15 @@ This package contains: ## Configuration Files - [`lights.xml`](./behavior_trees/lights.xml): BehaviorTree for managing lights. -- [`PantherLightsBT.btproj`](./behavior_trees/PantherLightsBT.btproj): BehaviorTree project for managing Panther lights. -- [`PantherSafetyBT.btproj`](./behavior_trees/PantherSafetyBT.btproj): BehaviorTree project for managing Panther safety protocols. +- [`LightsBT.btproj`](./behavior_trees/LightsBT.btproj): BehaviorTree project for managing Panther lights. +- [`SafetyBT.btproj`](./behavior_trees/SafetyBT.btproj): BehaviorTree project for managing Panther safety protocols. - [`safety.xml`](./behavior_trees/safety.xml): BehaviorTree for monitoring and managing dangerous situations. - [`shutdown.xml`](./behavior_trees/shutdown.xml): BehaviorTree for initiating shutdown procedures. +- [`lights_manager_parameters.yaml`](./config/lights_manager_parameters.yaml): Defines parameters for the `lights_manager` node. - [`lights_manager.yaml`](./config/lights_manager.yaml): Contains parameters for the `lights_manager` node. +- [`safety_manager_parameters.yaml`](./config/safety_manager_parameters.yaml): Defines parameters for the `safety_manager` node. - [`safety_manager.yaml`](./config/safety_manager.yaml): Contains parameters for the `safety_manager` node. -- [`shutdown_hosts_config.yaml`](./config/shutdown_hosts_config.yaml): List with all hosts to request shutdown. +- [`shutdown_hosts.yaml`](./config/shutdown_hosts.yaml): List with all hosts to request shutdown. ## ROS Nodes @@ -42,7 +44,7 @@ Node responsible for managing Bumper Lights animation scheduling. - `battery.percent.threshold.critical` [*float*, default: **0.1**]: If the Battery percentage drops below this value, an animation indicating a critical Battery state will start being displayed. - `battery.percent.threshold.low` [*float*, default: **0.4**]: If the Battery percentage drops below this value, the animation indicating a low Battery state will start being displayed. - `battery.percent.window_len` [*int*, default: **6**]: Moving average window length used to smooth out Battery percentage readings. -- `bt_project_path` [*string*, default: **$(find panther_manager)/config/PantherBT.btproj**]: Path to a BehaviorTree project. +- `bt_project_path` [*string*, default: **$(find husarion_ugv_manager)/config/PantherBT.btproj**]: Path to a BehaviorTree project. - `plugin_libs` [*list*, default: **Empty list**]: List with names of plugins that are used in the BT project. - `ros_communication_timeout.availability` [*float*, default: **1.0**]: Timeout **[s]** to wait for a service/action while initializing BT node. - `ros_communication_timeout.response` [*float*, default: **1.0**]: Timeout **[s]** to receive a service/action response after call. @@ -58,7 +60,7 @@ Node responsible for managing safety features, and software shutdown of componen - `battery/battery_status` [*sensor_msgs/BatteryState*]: State of the internal Battery. - `hardware/e_stop` [*std_msgs/Bool*]: State of emergency stop. - `hardware/io_state` [*panther_msgs/IOState*]: State of IO pins. -- `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: State of motor controllers. +- `hardware/robot_driver_state` [*panther_msgs/RobotDriverState*]: State of motor controllers. - `system_status` [*panther_msgs/SystemStatus*]: Built-in computer system status, includes the most important computation-related parameters. #### Service Clients (for Default Trees) @@ -70,7 +72,7 @@ Node responsible for managing safety features, and software shutdown of componen #### Parameters - `battery.temp.window_len` [*int*, default: **6**]: Moving average window length used to smooth out temperature readings of the Battery. -- `bt_project_path` [*string*, default: **$(find panther_manager)/config/PantherBT.btproj**]: Path to a BehaviorTree project. +- `bt_project_path` [*string*, default: **$(find husarion_ugv_manager)/config/PantherBT.btproj**]: Path to a BehaviorTree project. - `cpu.temp.fan_off` [*float*, default: **60.0**]: Temperature in **[°C]** of the Built-in Computer's CPU, below which the fan is turned off. - `cpu.temp.fan_on` [*float*, default: **70.0**]: Temperature in **[°C]** of the Built-in Computer's CPU, above which the fan is turned on. - `cpu.temp.window_len` [*int*, default: **6**]: Moving average window length used to smooth out temperature readings of the Built-in Computer's CPU. diff --git a/panther_manager/behavior_trees/PantherLightsBT.btproj b/husarion_ugv_manager/behavior_trees/LightsBT.btproj similarity index 93% rename from panther_manager/behavior_trees/PantherLightsBT.btproj rename to husarion_ugv_manager/behavior_trees/LightsBT.btproj index ab0f59aa..61bba3ef 100644 --- a/panther_manager/behavior_trees/PantherLightsBT.btproj +++ b/husarion_ugv_manager/behavior_trees/LightsBT.btproj @@ -1,5 +1,5 @@ - + diff --git a/panther_manager/behavior_trees/PantherSafetyBT.btproj b/husarion_ugv_manager/behavior_trees/SafetyBT.btproj similarity index 97% rename from panther_manager/behavior_trees/PantherSafetyBT.btproj rename to husarion_ugv_manager/behavior_trees/SafetyBT.btproj index 4ff1836e..b7f2bbc5 100644 --- a/panther_manager/behavior_trees/PantherSafetyBT.btproj +++ b/husarion_ugv_manager/behavior_trees/SafetyBT.btproj @@ -1,5 +1,5 @@ - + diff --git a/panther_manager/behavior_trees/lights.xml b/husarion_ugv_manager/behavior_trees/lights.xml similarity index 100% rename from panther_manager/behavior_trees/lights.xml rename to husarion_ugv_manager/behavior_trees/lights.xml diff --git a/panther_manager/behavior_trees/safety.xml b/husarion_ugv_manager/behavior_trees/safety.xml similarity index 100% rename from panther_manager/behavior_trees/safety.xml rename to husarion_ugv_manager/behavior_trees/safety.xml diff --git a/panther_manager/behavior_trees/shutdown.xml b/husarion_ugv_manager/behavior_trees/shutdown.xml similarity index 100% rename from panther_manager/behavior_trees/shutdown.xml rename to husarion_ugv_manager/behavior_trees/shutdown.xml diff --git a/panther_manager/config/lights_manager.yaml b/husarion_ugv_manager/config/lights_manager.yaml similarity index 100% rename from panther_manager/config/lights_manager.yaml rename to husarion_ugv_manager/config/lights_manager.yaml diff --git a/husarion_ugv_manager/config/lights_manager_parameters.yaml b/husarion_ugv_manager/config/lights_manager_parameters.yaml new file mode 100644 index 00000000..efe28cfd --- /dev/null +++ b/husarion_ugv_manager/config/lights_manager_parameters.yaml @@ -0,0 +1,74 @@ +lights_manager: + battery: + charging_anim_step: + type: double + default_value: 0.1 + description: This parameter defines the minimum change in battery percentage required to trigger an update in the battery charging animation. + validation: { gt<>: 0.0 } + + anim_period: + critical: + type: double + default_value: 15.0 + description: Time in seconds to wait before repeating animation, indicating a critical battery state. + validation: { gt<>: 0.0 } + + low: + type: double + default_value: 30.0 + description: Time in seconds to wait before repeating the animation, indicating a low battery state. + validation: { gt<>: 0.0 } + + percent: + window_len: + type: int + default_value: 6 + description: Moving average window length used to smooth out battery percentage readings. + validation: { gt<>: 0 } + + threshold: + critical: + type: double + default_value: 0.1 + description: If the battery percentage drops below this value, an animation indicating a critical battery state will start being displayed. + validation: { bounds<>: [0.0, 1.0] } + + low: + type: double + default_value: 0.4 + description: If the battery percentage drops below this value, the animation indicating a low battery state will start being displayed. + validation: { bounds<>: [0.0, 1.0] } + + bt_project_path: + type: string + default_value: "" + description: Path to a BehaviorTree project. + + plugin_libs: + type: string_array + default_value: [] + description: A list with names of plugins that are used in the BehaviorTree project. + + ros_communication_timeout: + availability: + type: double + default_value: 1.0 + description: Timeout in seconds to wait for a service/action while initializing a BehaviorTree node. + validation: { gt<>: 0.0 } + + response: + type: double + default_value: 1.0 + description: Timeout in seconds to receive a service/action response after call. + validation: { gt<>: 0.0 } + + ros_plugin_libs: + type: string_array + default_value: [] + description: A list with names of ROS plugins that are used in a BehaviorTree project. + + timer_frequency: + type: double + default_value: 10.0 + description: Frequency in Hz at which lights tree will be ticked. + validation: { gt<>: 0.0 } diff --git a/panther_manager/config/safety_manager.yaml b/husarion_ugv_manager/config/safety_manager.yaml similarity index 100% rename from panther_manager/config/safety_manager.yaml rename to husarion_ugv_manager/config/safety_manager.yaml diff --git a/husarion_ugv_manager/config/safety_manager_parameters.yaml b/husarion_ugv_manager/config/safety_manager_parameters.yaml new file mode 100644 index 00000000..068a4d9d --- /dev/null +++ b/husarion_ugv_manager/config/safety_manager_parameters.yaml @@ -0,0 +1,92 @@ +safety_manager: + battery: + temp: + window_len: + type: int + default_value: 6 + description: Moving average window length used to smooth out temperature readings of the battery. + validation: { gt<>: 0 } + + bt_project_path: + type: string + default_value: "" + description: Path to a BehaviorTree project. + + cpu: + temp: + fan_off: + type: double + default_value: 60.0 + description: Temperature in degrees Celsius of the Built-in Computer's CPU, below which the fan is turned off. + validation: { bounds<>: [-20.0, 100.0] } + + fan_on: + type: double + default_value: 70.0 + description: Temperature in degrees Celsius of the Built-in Computer's CPU, above which the fan is turned on. + validation: { bounds<>: [-20.0, 100.0] } + + window_len: + type: int + default_value: 6 + description: Moving average window length used to smooth out temperature readings of the Built-in Computer's CPU. + validation: { gt<>: 0 } + + driver: + temp: + fan_off: + type: double + default_value: 35.0 + description: Temperature in degrees Celsius of any drivers below which the fan is turned off. + validation: { bounds<>: [-20.0, 100.0] } + + fan_on: + type: double + default_value: 45.0 + description: Temperature in degrees Celsius of any drivers above which the fan is turned on. + validation: { bounds<>: [-20.0, 100.0] } + + window_len: + type: int + default_value: 6 + description: Moving average window length used to smooth out the temperature readings of each driver. + validation: { gt<>: 0 } + + fan_turn_off_timeout: + type: double + default_value: 60.0 + description: Minimal time in seconds, after which the fan may be turned off. + validation: { gt<>: 0.0 } + + plugin_libs: + type: string_array + default_value: [] + description: A list with names of plugins that are used in the a BehaviorTree project. + + ros_communication_timeout: + availability: + type: double + default_value: 1.0 + description: Timeout seconds to wait for a service/action while initializing a BehaviorTree node. + validation: { gt<>: 0.0 } + + response: + type: double + default_value: 1.0 + description: Timeout seconds to receive a service/action response after call. + validation: { gt<>: 0.0 } + + ros_plugin_libs: + type: string_array + default_value: [] + description: A list with names of ROS plugins that are used in a BehaviorTree project. + + shutdown_hosts_path: + type: string + default_value: "" + description: Path to a YAML file containing a list of hosts to request shutdown. To correctly format the YAML file, consult the README. + + timer_frequency: + type: double + default_value: 10.0 + description: Frequency in Hz at which safety tree will be ticked. diff --git a/panther_manager/config/shutdown_hosts_config.yaml b/husarion_ugv_manager/config/shutdown_hosts.yaml similarity index 100% rename from panther_manager/config/shutdown_hosts_config.yaml rename to husarion_ugv_manager/config/shutdown_hosts.yaml diff --git a/panther_manager/include/panther_manager/behavior_tree_manager.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_manager.hpp similarity index 92% rename from panther_manager/include/panther_manager/behavior_tree_manager.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_manager.hpp index 24d6eed0..831a0c7b 100644 --- a/panther_manager/include/panther_manager/behavior_tree_manager.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_ -#define PANTHER_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_ +#ifndef HUSARION_UGV_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_ +#define HUSARION_UGV_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_ #include #include @@ -24,7 +24,7 @@ #include "behaviortree_cpp/loggers/groot2_publisher.h" #include "rclcpp/rclcpp.hpp" -namespace panther_manager +namespace husarion_ugv_manager { /** @@ -93,6 +93,6 @@ class BehaviorTreeManager std::unique_ptr groot_publisher_; }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_ +#endif // HUSARION_UGV_MANAGER_BEHAVIOR_TREE_MANAGER_HPP_ diff --git a/panther_manager/include/panther_manager/behavior_tree_utils.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp similarity index 89% rename from panther_manager/include/panther_manager/behavior_tree_utils.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp index 360e174e..8d411136 100644 --- a/panther_manager/include/panther_manager/behavior_tree_utils.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_ -#define PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_ +#ifndef HUSARION_UGV_MANAGER_BEHAVIOR_TREE_UTILS_HPP_ +#define HUSARION_UGV_MANAGER_BEHAVIOR_TREE_UTILS_HPP_ #include #include @@ -26,7 +26,7 @@ #include "behaviortree_cpp/utils/shared_library.h" #include "behaviortree_ros2/plugins.hpp" -namespace panther_manager::behavior_tree_utils +namespace husarion_ugv_manager::behavior_tree_utils { /** @@ -73,9 +73,9 @@ inline void RegisterBehaviorTree( RegisterBehaviorTree(factory, bt_project_path, plugin_libs); } -} // namespace panther_manager::behavior_tree_utils +} // namespace husarion_ugv_manager::behavior_tree_utils -namespace panther_manager +namespace husarion_ugv_manager { // TODO: @pawelirh move to a separate file with an appropriate abstraction layer /** @@ -88,6 +88,6 @@ inline std::string GetLoggerPrefix(const std::string & bt_node_name) { return std::string("[" + bt_node_name + "] "); } -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_ +#endif // HUSARION_UGV_MANAGER_BEHAVIOR_TREE_UTILS_HPP_ diff --git a/panther_manager/include/panther_manager/lights_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp similarity index 80% rename from panther_manager/include/panther_manager/lights_manager_node.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp index 21937cd8..655e19e7 100644 --- a/panther_manager/include/panther_manager/lights_manager_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_LIGHTS_MANAGER_NODE_HPP_ -#define PANTHER_MANAGER_LIGHTS_MANAGER_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_LIGHTS_MANAGER_NODE_HPP_ +#define HUSARION_UGV_MANAGER_LIGHTS_MANAGER_NODE_HPP_ #include #include @@ -26,11 +26,13 @@ #include "panther_msgs/msg/led_animation.hpp" -#include "panther_utils/moving_average.hpp" +#include "lights_manager_parameters.hpp" -#include +#include "husarion_ugv_utils/moving_average.hpp" -namespace panther_manager +#include + +namespace husarion_ugv_manager { using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -73,14 +75,17 @@ class LightsManagerNode : public rclcpp::Node float update_charging_anim_step_; + std::shared_ptr param_listener_; + lights_manager::Params params_; + rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr e_stop_sub_; rclcpp::TimerBase::SharedPtr lights_tree_timer_; - std::unique_ptr> battery_percent_ma_; + std::unique_ptr> battery_percent_ma_; BT::BehaviorTreeFactory factory_; }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_LIGHTS_MANAGER_NODE_HPP_ +#endif // HUSARION_UGV_MANAGER_LIGHTS_MANAGER_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp similarity index 81% rename from panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp index adafe1d4..f9bfae2e 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_ -#define PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_ #include @@ -22,7 +22,7 @@ #include "std_srvs/srv/set_bool.hpp" -namespace panther_manager +namespace husarion_ugv_manager { class CallSetBoolService : public BT::RosServiceNode @@ -43,6 +43,6 @@ class CallSetBoolService : public BT::RosServiceNode virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override; }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#endif // HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp similarity index 82% rename from panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp index 38a915a5..4fa2f8f8 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ -#define PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ #include @@ -22,7 +22,7 @@ #include "panther_msgs/srv/set_led_animation.hpp" -namespace panther_manager +namespace husarion_ugv_manager { class CallSetLedAnimationService : public BT::RosServiceNode @@ -47,6 +47,6 @@ class CallSetLedAnimationService : public BT::RosServiceNode @@ -22,7 +22,7 @@ #include "std_srvs/srv/trigger.hpp" -namespace panther_manager +namespace husarion_ugv_manager { class CallTriggerService : public BT::RosServiceNode @@ -40,6 +40,6 @@ class CallTriggerService : public BT::RosServiceNode virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override; }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_PLUGINS_ACTION_CALL_TRIGGER_SERVICE_NODE_HPP_ +#endif // HUSARION_UGV_MANAGER_PLUGINS_ACTION_CALL_TRIGGER_SERVICE_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp similarity index 71% rename from panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp index 949565f6..10396a8c 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ -#define PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ #include #include @@ -22,11 +22,11 @@ #include "behaviortree_cpp/basic_types.h" #include "yaml-cpp/yaml.h" -#include "panther_manager/plugins/shutdown_host.hpp" -#include "panther_manager/plugins/shutdown_hosts_node.hpp" -#include "panther_utils/yaml_utils.hpp" +#include "husarion_ugv_manager/plugins/shutdown_host.hpp" +#include "husarion_ugv_manager/plugins/shutdown_hosts_node.hpp" +#include "husarion_ugv_utils/yaml_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { class ShutdownHostsFromFile : public ShutdownHosts @@ -49,6 +49,6 @@ class ShutdownHostsFromFile : public ShutdownHosts bool UpdateHosts(std::vector> & hosts) override; }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ +#endif // HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp similarity index 78% rename from panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp index 630db650..be36a7f9 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_ -#define PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_ #include #include @@ -21,10 +21,10 @@ #include "behaviortree_cpp/basic_types.h" -#include "panther_manager/plugins/shutdown_host.hpp" -#include "panther_manager/plugins/shutdown_hosts_node.hpp" +#include "husarion_ugv_manager/plugins/shutdown_host.hpp" +#include "husarion_ugv_manager/plugins/shutdown_hosts_node.hpp" -namespace panther_manager +namespace husarion_ugv_manager { class ShutdownSingleHost : public ShutdownHosts @@ -52,6 +52,6 @@ class ShutdownSingleHost : public ShutdownHosts bool UpdateHosts(std::vector> & hosts) override; }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_ +#endif // HUSARION_UGV_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp similarity index 79% rename from panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp index 7bc9709d..8e98387e 100644 --- a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_ -#define PANTHER_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_ #include @@ -21,7 +21,7 @@ #include "behaviortree_cpp/basic_types.h" #include "behaviortree_cpp/tree_node.h" -namespace panther_manager +namespace husarion_ugv_manager { class SignalShutdown : public BT::SyncActionNode @@ -42,6 +42,6 @@ class SignalShutdown : public BT::SyncActionNode virtual BT::NodeStatus tick() override; }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_ +#endif // HUSARION_UGV_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp similarity index 80% rename from panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp index 20a273e5..a453054f 100644 --- a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_ -#define PANTHER_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_ #include #include @@ -22,7 +22,7 @@ #include "behaviortree_cpp/decorator_node.h" #include "behaviortree_cpp/tree_node.h" -namespace panther_manager +namespace husarion_ugv_manager { class TickAfterTimeout : public BT::DecoratorNode { @@ -40,6 +40,6 @@ class TickAfterTimeout : public BT::DecoratorNode BT::NodeStatus tick() override; }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_ +#endif // HUSARION_UGV_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_host.hpp similarity index 96% rename from panther_manager/include/panther_manager/plugins/shutdown_host.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_host.hpp index ce29367a..6f4729ba 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_host.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_ -#define PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_ #include #include @@ -25,7 +25,7 @@ #include "libssh/libsshpp.hpp" -namespace panther_manager +namespace husarion_ugv_manager { enum class ShutdownHostState { @@ -268,6 +268,6 @@ class ShutdownHost } }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_ +#endif // HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_hosts_node.hpp similarity index 93% rename from panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_hosts_node.hpp index 004c94a4..5c7b11ad 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/shutdown_hosts_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_ -#define PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_ #include #include @@ -29,11 +29,11 @@ #include "behaviortree_cpp/tree_node.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/plugins/shutdown_host.hpp" +#include "husarion_ugv_manager/plugins/shutdown_host.hpp" -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { class ShutdownHosts : public BT::StatefulActionNode @@ -177,6 +177,6 @@ class ShutdownHosts : public BT::StatefulActionNode } }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_ +#endif // HUSARION_UGV_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/safety_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp similarity index 73% rename from panther_manager/include/panther_manager/safety_manager_node.hpp rename to husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp index fcfcf683..3d2b247b 100644 --- a/panther_manager/include/panther_manager/safety_manager_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_SAFETY_MANAGER_NODE_HPP_ -#define PANTHER_MANAGER_SAFETY_MANAGER_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_SAFETY_MANAGER_NODE_HPP_ +#define HUSARION_UGV_MANAGER_SAFETY_MANAGER_NODE_HPP_ +#include #include #include @@ -24,20 +25,22 @@ #include "sensor_msgs/msg/battery_state.hpp" #include "std_msgs/msg/bool.hpp" -#include "panther_msgs/msg/driver_state.hpp" #include "panther_msgs/msg/io_state.hpp" +#include "panther_msgs/msg/robot_driver_state.hpp" #include "panther_msgs/msg/system_status.hpp" -#include "panther_utils/moving_average.hpp" +#include "safety_manager_parameters.hpp" -#include +#include "husarion_ugv_utils/moving_average.hpp" -namespace panther_manager +#include + +namespace husarion_ugv_manager { using BatteryStateMsg = sensor_msgs::msg::BatteryState; using BoolMsg = std_msgs::msg::Bool; -using DriverStateMsg = panther_msgs::msg::DriverState; +using RobotDriverStateMsg = panther_msgs::msg::RobotDriverState; using IOStateMsg = panther_msgs::msg::IOState; using SystemStatusMsg = panther_msgs::msg::SystemStatus; @@ -74,7 +77,7 @@ class SafetyManagerNode : public rclcpp::Node private: void BatteryCB(const BatteryStateMsg::SharedPtr battery); - void DriverStateCB(const DriverStateMsg::SharedPtr driver_state); + void RobotDriverStateCB(const RobotDriverStateMsg::SharedPtr driver_state); void EStopCB(const BoolMsg::SharedPtr e_stop); void IOStateCB(const IOStateMsg::SharedPtr io_state); void SystemStatusCB(const SystemStatusMsg::SharedPtr system_status); @@ -85,21 +88,25 @@ class SafetyManagerNode : public rclcpp::Node static constexpr float kCriticalBatteryTemp = 55.0; static constexpr float kFatalBatteryTemp = 62.0; + int driver_temp_window_len_; float update_charging_anim_step_; + std::shared_ptr param_listener_; + safety_manager::Params params_; + rclcpp::Subscription::SharedPtr battery_sub_; - rclcpp::Subscription::SharedPtr driver_state_sub_; + rclcpp::Subscription::SharedPtr driver_state_sub_; rclcpp::Subscription::SharedPtr e_stop_sub_; rclcpp::Subscription::SharedPtr io_state_sub_; rclcpp::Subscription::SharedPtr system_status_sub_; rclcpp::TimerBase::SharedPtr safety_tree_timer_; - std::unique_ptr> battery_temp_ma_; - std::unique_ptr> cpu_temp_ma_; - std::unique_ptr> front_driver_temp_ma_; - std::unique_ptr> rear_driver_temp_ma_; + std::unique_ptr> battery_temp_ma_; + std::unique_ptr> cpu_temp_ma_; + + std::map>> driver_temp_ma_; }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_SAFETY_MANAGER_NODE_HPP_ +#endif // HUSARION_UGV_MANAGER_SAFETY_MANAGER_NODE_HPP_ diff --git a/panther_manager/launch/manager.launch.py b/husarion_ugv_manager/launch/manager.launch.py similarity index 78% rename from panther_manager/launch/manager.launch.py rename to husarion_ugv_manager/launch/manager.launch.py index 14f4c8c7..60c0b4f5 100644 --- a/panther_manager/launch/manager.launch.py +++ b/husarion_ugv_manager/launch/manager.launch.py @@ -16,12 +16,11 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition +from launch.conditions import UnlessCondition from launch.substitutions import ( EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution, - PythonExpression, ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -29,14 +28,14 @@ def generate_launch_description(): - panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0") - panther_manager_dir = FindPackageShare("panther_manager") + husarion_ugv_manager_pkg = FindPackageShare("husarion_ugv_manager") + husarion_ugv_manager_common_dir = PathJoinSubstitution(["/config", "husarion_ugv_manager"]) lights_bt_project_path = LaunchConfiguration("lights_bt_project_path") declare_lights_bt_project_path_arg = DeclareLaunchArgument( "lights_bt_project_path", default_value=PathJoinSubstitution( - [panther_manager_dir, "behavior_trees", "PantherLightsBT.btproj"] + [husarion_ugv_manager_common_dir, "behavior_trees", "LightsBT.btproj"] ), description="Path to BehaviorTree project file, responsible for lights management.", ) @@ -52,10 +51,9 @@ def generate_launch_description(): declare_safety_bt_project_path_arg = DeclareLaunchArgument( "safety_bt_project_path", default_value=PathJoinSubstitution( - [panther_manager_dir, "behavior_trees", "PantherSafetyBT.btproj"] + [husarion_ugv_manager_pkg, "behavior_trees", "SafetyBT.btproj"] ), description="Path to BehaviorTree project file, responsible for safety and shutdown management.", - condition=IfCondition(PythonExpression([panther_version, ">=", "1.2"])), ) shutdown_hosts_config_path = LaunchConfiguration("shutdown_hosts_config_path") @@ -63,9 +61,9 @@ def generate_launch_description(): "shutdown_hosts_config_path", default_value=PathJoinSubstitution( [ - FindPackageShare("panther_manager"), + husarion_ugv_manager_common_dir, "config", - "shutdown_hosts_config.yaml", + "shutdown_hosts.yaml", ] ), description="Path to file with list of hosts to request shutdown.", @@ -79,11 +77,11 @@ def generate_launch_description(): ) lights_manager_node = Node( - package="panther_manager", + package="husarion_ugv_manager", executable="lights_manager_node", name="lights_manager", parameters=[ - PathJoinSubstitution([panther_manager_dir, "config", "lights_manager.yaml"]), + PathJoinSubstitution([husarion_ugv_manager_pkg, "config", "lights_manager.yaml"]), {"bt_project_path": lights_bt_project_path}, ], namespace=namespace, @@ -91,11 +89,11 @@ def generate_launch_description(): ) safety_manager_node = Node( - package="panther_manager", + package="husarion_ugv_manager", executable="safety_manager_node", name="safety_manager", parameters=[ - PathJoinSubstitution([panther_manager_dir, "config", "safety_manager.yaml"]), + PathJoinSubstitution([husarion_ugv_manager_pkg, "config", "safety_manager.yaml"]), { "bt_project_path": safety_bt_project_path, "shutdown_hosts_path": shutdown_hosts_config_path, @@ -103,7 +101,7 @@ def generate_launch_description(): ], namespace=namespace, emulate_tty=True, - condition=IfCondition(PythonExpression([panther_version, ">=", "1.2 and not ", use_sim])), + condition=UnlessCondition(use_sim), ) actions = [ diff --git a/panther_manager/package.xml b/husarion_ugv_manager/package.xml similarity index 89% rename from panther_manager/package.xml rename to husarion_ugv_manager/package.xml index 5acc77e6..31972d27 100644 --- a/panther_manager/package.xml +++ b/husarion_ugv_manager/package.xml @@ -1,10 +1,10 @@ - panther_manager + husarion_ugv_manager 2.1.2 - Set of nodes used for high level management of Husarion Panther robot + Set of nodes used for high level management of Husarion UGV Husarion Apache License 2.0 @@ -19,11 +19,12 @@ behaviortree_cpp behaviortree_ros2 + generate_parameter_library + husarion_ugv_utils iputils-ping libboost-dev libssh-dev panther_msgs - panther_utils rclcpp rclcpp_action std_srvs diff --git a/panther_manager/src/behavior_tree_manager.cpp b/husarion_ugv_manager/src/behavior_tree_manager.cpp similarity index 94% rename from panther_manager/src/behavior_tree_manager.cpp rename to husarion_ugv_manager/src/behavior_tree_manager.cpp index f39a3646..c5beb1a4 100644 --- a/panther_manager/src/behavior_tree_manager.cpp +++ b/husarion_ugv_manager/src/behavior_tree_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -24,7 +24,7 @@ #include "behaviortree_cpp/loggers/groot2_publisher.h" #include "rclcpp/rclcpp.hpp" -namespace panther_manager +namespace husarion_ugv_manager { void BehaviorTreeManager::Initialize(BT::BehaviorTreeFactory & factory) @@ -66,4 +66,4 @@ BT::NodeConfig BehaviorTreeManager::CreateBTConfig( return config; } -} // namespace panther_manager +} // namespace husarion_ugv_manager diff --git a/panther_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp similarity index 70% rename from panther_manager/src/lights_manager_node.cpp rename to husarion_ugv_manager/src/lights_manager_node.cpp index 53ff612b..2238ded1 100644 --- a/panther_manager/src/lights_manager_node.cpp +++ b/husarion_ugv_manager/src/lights_manager_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/lights_manager_node.hpp" +#include "husarion_ugv_manager/lights_manager_node.hpp" #include #include @@ -22,16 +22,15 @@ #include #include -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behaviortree_ros2/ros_node_params.hpp" #include "rclcpp/rclcpp.hpp" -#include "panther_utils/moving_average.hpp" +#include "husarion_ugv_utils/moving_average.hpp" -#include -#include +#include +#include -namespace panther_manager +namespace husarion_ugv_manager { LightsManagerNode::LightsManagerNode( @@ -40,12 +39,13 @@ LightsManagerNode::LightsManagerNode( { RCLCPP_INFO(this->get_logger(), "Constructing node."); - DeclareParameters(); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); - const auto battery_percent_window_len = - this->get_parameter("battery.percent.window_len").as_int(); + const auto battery_percent_window_len = this->params_.battery.percent.window_len; - battery_percent_ma_ = std::make_unique>( + battery_percent_ma_ = std::make_unique>( battery_percent_window_len, 1.0); const auto initial_blackboard = CreateLightsInitialBlackboard(); @@ -69,7 +69,7 @@ void LightsManagerNode::Initialize() "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&LightsManagerNode::EStopCB, this, _1)); - const float timer_freq = this->get_parameter("timer_frequency").as_double(); + const double timer_freq = this->params_.timer_frequency; const auto timer_period_ms = std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000)); @@ -79,40 +79,15 @@ void LightsManagerNode::Initialize() RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void LightsManagerNode::DeclareParameters() -{ - const auto panther_manager_pkg_path = - ament_index_cpp::get_package_share_directory("panther_manager"); - const std::string default_bt_project_path = panther_manager_pkg_path + - "/behavior_trees/PantherLightsBT.btproj"; - const std::vector default_plugin_libs = {}; - - this->declare_parameter("bt_project_path", default_bt_project_path); - this->declare_parameter>("plugin_libs", default_plugin_libs); - this->declare_parameter>("ros_plugin_libs", default_plugin_libs); - this->declare_parameter("ros_communication_timeout.availability", 1.0); - this->declare_parameter("ros_communication_timeout.response", 1.0); - - this->declare_parameter("battery.percent.window_len", 6); - this->declare_parameter("battery.percent.threshold.low", 0.4); - this->declare_parameter("battery.percent.threshold.critical", 0.1); - this->declare_parameter("battery.animation_period.low", 30.0); - this->declare_parameter("battery.animation_period.critical", 15.0); - this->declare_parameter("battery.charging_anim_step", 0.1); - this->declare_parameter("timer_frequency", 10.0); -} - void LightsManagerNode::RegisterBehaviorTree() { - const auto bt_project_path = this->get_parameter("bt_project_path").as_string(); + const auto bt_project_path = this->params_.bt_project_path; - const auto plugin_libs = this->get_parameter("plugin_libs").as_string_array(); - const auto ros_plugin_libs = this->get_parameter("ros_plugin_libs").as_string_array(); + const auto plugin_libs = this->params_.plugin_libs; + const auto ros_plugin_libs = this->params_.ros_plugin_libs; - const auto service_availability_timeout = - this->get_parameter("ros_communication_timeout.availability").as_double(); - const auto service_response_timeout = - this->get_parameter("ros_communication_timeout.response").as_double(); + const auto service_availability_timeout = this->params_.ros_communication_timeout.availability; + const auto service_response_timeout = this->params_.ros_communication_timeout.response; BT::RosNodeParams params; params.nh = this->shared_from_this(); @@ -130,15 +105,14 @@ void LightsManagerNode::RegisterBehaviorTree() std::map LightsManagerNode::CreateLightsInitialBlackboard() { - update_charging_anim_step_ = this->get_parameter("battery.charging_anim_step").as_double(); + update_charging_anim_step_ = this->params_.battery.charging_anim_step; const float critical_battery_anim_period = - this->get_parameter("battery.animation_period.critical").as_double(); + static_cast(this->params_.battery.anim_period.critical); const float critical_battery_threshold_percent = - this->get_parameter("battery.percent.threshold.critical").as_double(); - const float low_battery_anim_period = - this->get_parameter("battery.animation_period.low").as_double(); + static_cast(this->params_.battery.percent.threshold.critical); + const float low_battery_anim_period = static_cast(this->params_.battery.anim_period.low); const float low_battery_threshold_percent = - this->get_parameter("battery.percent.threshold.low").as_double(); + static_cast(this->params_.battery.percent.threshold.low); const std::string undefined_charging_anim_percent = ""; const int undefined_anim_id = -1; @@ -232,4 +206,4 @@ bool LightsManagerNode::SystemReady() return true; } -} // namespace panther_manager +} // namespace husarion_ugv_manager diff --git a/panther_manager/src/lights_manager_node_main.cpp b/husarion_ugv_manager/src/lights_manager_node_main.cpp similarity index 86% rename from panther_manager/src/lights_manager_node_main.cpp rename to husarion_ugv_manager/src/lights_manager_node_main.cpp index 92a2738b..9fbbc3c8 100644 --- a/panther_manager/src/lights_manager_node_main.cpp +++ b/husarion_ugv_manager/src/lights_manager_node_main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/lights_manager_node.hpp" +#include "husarion_ugv_manager/lights_manager_node.hpp" #include #include @@ -22,7 +22,8 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto lights_manager_node = std::make_shared("lights_manager"); + auto lights_manager_node = + std::make_shared("lights_manager"); lights_manager_node->Initialize(); try { diff --git a/panther_manager/src/plugins/action/call_set_bool_service_node.cpp b/husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp similarity index 84% rename from panther_manager/src/plugins/action/call_set_bool_service_node.cpp rename to husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp index 561e2a5f..b51b58b8 100644 --- a/panther_manager/src/plugins/action/call_set_bool_service_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/action/call_set_bool_service_node.hpp" +#include "husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp" -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { bool CallSetBoolService::setRequest(typename Request::SharedPtr & request) @@ -42,7 +42,7 @@ BT::NodeStatus CallSetBoolService::onResponseReceived(const typename Response::S return BT::NodeStatus::SUCCESS; } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(panther_manager::CallSetBoolService, "CallSetBoolService"); +CreateRosNodePlugin(husarion_ugv_manager::CallSetBoolService, "CallSetBoolService"); diff --git a/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp b/husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp similarity index 86% rename from panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp rename to husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp index b0f22dd8..70c07642 100644 --- a/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/action/call_set_led_animation_service_node.hpp" +#include "husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp" #include #include "behaviortree_cpp/basic_types.h" -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { bool CallSetLedAnimationService::setRequest(typename Request::SharedPtr & request) @@ -62,7 +62,7 @@ BT::NodeStatus CallSetLedAnimationService::onResponseReceived( return BT::NodeStatus::SUCCESS; } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(panther_manager::CallSetLedAnimationService, "CallSetLedAnimationService"); +CreateRosNodePlugin(husarion_ugv_manager::CallSetLedAnimationService, "CallSetLedAnimationService"); diff --git a/panther_manager/src/plugins/action/call_trigger_service_node.cpp b/husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp similarity index 83% rename from panther_manager/src/plugins/action/call_trigger_service_node.cpp rename to husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp index e24b8463..73c70856 100644 --- a/panther_manager/src/plugins/action/call_trigger_service_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/action/call_trigger_service_node.hpp" +#include "husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp" #include "behaviortree_cpp/basic_types.h" -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { bool CallTriggerService::setRequest(typename Request::SharedPtr & /*request*/) { return true; } @@ -37,7 +37,7 @@ BT::NodeStatus CallTriggerService::onResponseReceived(const typename Response::S return BT::NodeStatus::SUCCESS; } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(panther_manager::CallTriggerService, "CallTriggerService"); +CreateRosNodePlugin(husarion_ugv_manager::CallTriggerService, "CallTriggerService"); diff --git a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp b/husarion_ugv_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp similarity index 71% rename from panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp rename to husarion_ugv_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp index 270c67a5..a8dcfb38 100644 --- a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp" +#include "husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp" #include #include @@ -24,10 +24,10 @@ #include "behaviortree_cpp/tree_node.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/behavior_tree_utils.hpp" -#include "panther_manager/plugins/shutdown_host.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/plugins/shutdown_host.hpp" -namespace panther_manager +namespace husarion_ugv_manager { bool ShutdownHostsFromFile::UpdateHosts(std::vector> & hosts) @@ -59,13 +59,13 @@ bool ShutdownHostsFromFile::UpdateHosts(std::vector(host, "ip"); - const auto username = panther_utils::GetYAMLKeyValue(host, "username"); - const auto port = panther_utils::GetYAMLKeyValue(host, "port", 22); - const auto command = panther_utils::GetYAMLKeyValue( + const auto ip = husarion_ugv_utils::GetYAMLKeyValue(host, "ip"); + const auto username = husarion_ugv_utils::GetYAMLKeyValue(host, "username"); + const auto port = husarion_ugv_utils::GetYAMLKeyValue(host, "port", 22); + const auto command = husarion_ugv_utils::GetYAMLKeyValue( host, "command", "sudo shutdown now"); - const auto timeout = panther_utils::GetYAMLKeyValue(host, "timeout", 5.0); - const auto ping_for_success = panther_utils::GetYAMLKeyValue( + const auto timeout = husarion_ugv_utils::GetYAMLKeyValue(host, "timeout", 5.0); + const auto ping_for_success = husarion_ugv_utils::GetYAMLKeyValue( host, "ping_for_success", true); hosts.push_back( std::make_shared(ip, username, port, command, timeout, ping_for_success)); @@ -77,10 +77,10 @@ bool ShutdownHostsFromFile::UpdateHosts(std::vector("ShutdownHostsFromFile"); + factory.registerNodeType("ShutdownHostsFromFile"); } diff --git a/panther_manager/src/plugins/action/shutdown_single_host_node.cpp b/husarion_ugv_manager/src/plugins/action/shutdown_single_host_node.cpp similarity index 86% rename from panther_manager/src/plugins/action/shutdown_single_host_node.cpp rename to husarion_ugv_manager/src/plugins/action/shutdown_single_host_node.cpp index 5c4f7eae..a2dbabb9 100644 --- a/panther_manager/src/plugins/action/shutdown_single_host_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/shutdown_single_host_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/action/shutdown_single_host_node.hpp" +#include "husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp" #include #include @@ -21,10 +21,10 @@ #include "behaviortree_cpp/exceptions.h" #include "behaviortree_cpp/tree_node.h" -#include "panther_manager/behavior_tree_utils.hpp" -#include "panther_manager/plugins/shutdown_host.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/plugins/shutdown_host.hpp" -namespace panther_manager +namespace husarion_ugv_manager { bool ShutdownSingleHost::UpdateHosts(std::vector> & hosts) @@ -71,10 +71,10 @@ bool ShutdownSingleHost::UpdateHosts(std::vector> return true; } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { - factory.registerNodeType("ShutdownSingleHost"); + factory.registerNodeType("ShutdownSingleHost"); } diff --git a/panther_manager/src/plugins/action/signal_shutdown_node.cpp b/husarion_ugv_manager/src/plugins/action/signal_shutdown_node.cpp similarity index 83% rename from panther_manager/src/plugins/action/signal_shutdown_node.cpp rename to husarion_ugv_manager/src/plugins/action/signal_shutdown_node.cpp index 8d674def..d9a5fd02 100644 --- a/panther_manager/src/plugins/action/signal_shutdown_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/signal_shutdown_node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/action/signal_shutdown_node.hpp" +#include "husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp" #include #include #include "behaviortree_cpp/basic_types.h" -namespace panther_manager +namespace husarion_ugv_manager { BT::NodeStatus SignalShutdown::tick() @@ -34,10 +34,10 @@ BT::NodeStatus SignalShutdown::tick() return BT::NodeStatus::SUCCESS; } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { - factory.registerNodeType("SignalShutdown"); + factory.registerNodeType("SignalShutdown"); } diff --git a/panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp b/husarion_ugv_manager/src/plugins/decorator/tick_after_timeout_node.cpp similarity index 87% rename from panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp rename to husarion_ugv_manager/src/plugins/decorator/tick_after_timeout_node.cpp index 8a746ec3..e89ebb9a 100644 --- a/panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp +++ b/husarion_ugv_manager/src/plugins/decorator/tick_after_timeout_node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/decorator/tick_after_timeout_node.hpp" +#include "husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp" -namespace panther_manager +namespace husarion_ugv_manager { TickAfterTimeout::TickAfterTimeout(const std::string & name, const BT::NodeConfig & conf) @@ -53,10 +53,10 @@ BT::NodeStatus TickAfterTimeout::tick() return child_status; } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { - factory.registerNodeType("TickAfterTimeout"); + factory.registerNodeType("TickAfterTimeout"); } diff --git a/panther_manager/src/safety_manager_node.cpp b/husarion_ugv_manager/src/safety_manager_node.cpp similarity index 68% rename from panther_manager/src/safety_manager_node.cpp rename to husarion_ugv_manager/src/safety_manager_node.cpp index 68ea0c19..33ede2c0 100644 --- a/panther_manager/src/safety_manager_node.cpp +++ b/husarion_ugv_manager/src/safety_manager_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/safety_manager_node.hpp" +#include "husarion_ugv_manager/safety_manager_node.hpp" #include #include @@ -24,16 +24,15 @@ #include #include -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behaviortree_ros2/ros_node_params.hpp" #include "rclcpp/rclcpp.hpp" -#include "panther_utils/moving_average.hpp" +#include "husarion_ugv_utils/moving_average.hpp" -#include -#include +#include +#include -namespace panther_manager +namespace husarion_ugv_manager { SafetyManagerNode::SafetyManagerNode( @@ -42,25 +41,22 @@ SafetyManagerNode::SafetyManagerNode( { RCLCPP_INFO(this->get_logger(), "Constructing node."); - DeclareParameters(); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); - const auto battery_temp_window_len = this->get_parameter("battery.temp.window_len").as_int(); - const auto cpu_temp_window_len = this->get_parameter("cpu.temp.window_len").as_int(); - const auto driver_temp_window_len = this->get_parameter("driver.temp.window_len").as_int(); + const auto battery_temp_window_len = this->params_.battery.temp.window_len; + const auto cpu_temp_window_len = this->params_.cpu.temp.window_len; battery_temp_ma_ = - std::make_unique>(battery_temp_window_len); - cpu_temp_ma_ = std::make_unique>(cpu_temp_window_len); - front_driver_temp_ma_ = - std::make_unique>(driver_temp_window_len); - rear_driver_temp_ma_ = - std::make_unique>(driver_temp_window_len); + std::make_unique>(battery_temp_window_len); + cpu_temp_ma_ = std::make_unique>(cpu_temp_window_len); const auto safety_initial_blackboard = CreateSafetyInitialBlackboard(); safety_tree_manager_ = std::make_unique( "Safety", safety_initial_blackboard, 6666); - const auto shutdown_hosts_path = this->get_parameter("shutdown_hosts_path").as_string(); + const auto shutdown_hosts_path = this->params_.shutdown_hosts_path; const std::map shutdown_initial_blackboard = { {"SHUTDOWN_HOSTS_FILE", shutdown_hosts_path}, }; @@ -86,8 +82,8 @@ void SafetyManagerNode::Initialize() battery_sub_ = this->create_subscription( "battery/battery_status", 10, std::bind(&SafetyManagerNode::BatteryCB, this, _1)); - driver_state_sub_ = this->create_subscription( - "hardware/motor_controllers_state", 10, std::bind(&SafetyManagerNode::DriverStateCB, this, _1)); + driver_state_sub_ = this->create_subscription( + "hardware/robot_driver_state", 10, std::bind(&SafetyManagerNode::RobotDriverStateCB, this, _1)); e_stop_sub_ = this->create_subscription( "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&SafetyManagerNode::EStopCB, this, _1)); @@ -101,7 +97,7 @@ void SafetyManagerNode::Initialize() // Timers // ------------------------------- - const float timer_freq = this->get_parameter("timer_frequency").as_double(); + const double timer_freq = this->params_.timer_frequency; const auto timer_period_ms = std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000)); @@ -111,43 +107,15 @@ void SafetyManagerNode::Initialize() RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void SafetyManagerNode::DeclareParameters() -{ - const auto panther_manager_pkg_path = - ament_index_cpp::get_package_share_directory("panther_manager"); - const std::string default_bt_project_path = panther_manager_pkg_path + - "/behavior_trees/PantherSafetyBT.btproj"; - const std::vector default_plugin_libs = {}; - - this->declare_parameter("bt_project_path", default_bt_project_path); - this->declare_parameter("shutdown_hosts_path", ""); - this->declare_parameter>("plugin_libs", default_plugin_libs); - this->declare_parameter>("ros_plugin_libs", default_plugin_libs); - this->declare_parameter("ros_communication_timeout.availability", 1.0); - this->declare_parameter("ros_communication_timeout.response", 1.0); - - this->declare_parameter("battery.temp.window_len", 6); - this->declare_parameter("cpu.temp.window_len", 6); - this->declare_parameter("cpu.temp.fan_on", 70.0); - this->declare_parameter("cpu.temp.fan_off", 60.0); - this->declare_parameter("driver.temp.window_len", 6); - this->declare_parameter("driver.temp.fan_on", 45.0); - this->declare_parameter("driver.temp.fan_off", 35.0); - this->declare_parameter("timer_frequency", 10.0); - this->declare_parameter("fan_turn_off_timeout", 60.0); -} - void SafetyManagerNode::RegisterBehaviorTree() { - const auto bt_project_path = this->get_parameter("bt_project_path").as_string(); + const auto bt_project_path = this->params_.bt_project_path; - const auto plugin_libs = this->get_parameter("plugin_libs").as_string_array(); - const auto ros_plugin_libs = this->get_parameter("ros_plugin_libs").as_string_array(); + const auto plugin_libs = this->params_.plugin_libs; + const auto ros_plugin_libs = this->params_.ros_plugin_libs; - const auto service_availability_timeout = - this->get_parameter("ros_communication_timeout.availability").as_double(); - const auto service_response_timeout = - this->get_parameter("ros_communication_timeout.response").as_double(); + const auto service_availability_timeout = this->params_.ros_communication_timeout.availability; + const auto service_response_timeout = this->params_.ros_communication_timeout.response; BT::RosNodeParams params; params.nh = this->shared_from_this(); @@ -165,11 +133,11 @@ void SafetyManagerNode::RegisterBehaviorTree() std::map SafetyManagerNode::CreateSafetyInitialBlackboard() { - const float cpu_fan_on_temp = this->get_parameter("cpu.temp.fan_on").as_double(); - const float cpu_fan_off_temp = this->get_parameter("cpu.temp.fan_off").as_double(); - const float driver_fan_on_temp = this->get_parameter("driver.temp.fan_on").as_double(); - const float driver_fan_off_temp = this->get_parameter("driver.temp.fan_off").as_double(); - const float fan_turn_off_timeout = this->get_parameter("fan_turn_off_timeout").as_double(); + const double cpu_fan_on_temp = this->params_.cpu.temp.fan_on; + const double cpu_fan_off_temp = this->params_.cpu.temp.fan_off; + const double driver_fan_on_temp = this->params_.driver.temp.fan_on; + const double driver_fan_off_temp = this->params_.driver.temp.fan_off; + const float fan_turn_off_timeout = static_cast(this->params_.fan_turn_off_timeout); const std::map safety_initial_bb = { {"CPU_FAN_OFF_TEMP", cpu_fan_off_temp}, @@ -215,15 +183,32 @@ void SafetyManagerNode::BatteryCB(const BatteryStateMsg::SharedPtr battery) safety_tree_manager_->GetBlackboard()->set("bat_temp", battery_temp_ma_->GetAverage()); } -void SafetyManagerNode::DriverStateCB(const DriverStateMsg::SharedPtr driver_state) +void SafetyManagerNode::RobotDriverStateCB(const RobotDriverStateMsg::SharedPtr driver_state) { - front_driver_temp_ma_->Roll(driver_state->front.temperature); - rear_driver_temp_ma_->Roll(driver_state->rear.temperature); + if (driver_state->driver_states.empty()) { + RCLCPP_WARN(this->get_logger(), "Received empty driver state message."); + return; + } + + for (auto & driver : driver_state->driver_states) { + if (driver_temp_ma_.find(driver.name) == driver_temp_ma_.end()) { + RCLCPP_DEBUG( + this->get_logger(), "Creating moving average for driver '%s'", driver.name.c_str()); + const auto driver_temp_window_len = this->params_.driver.temp.window_len; + driver_temp_ma_[driver.name] = + std::make_unique>(driver_temp_window_len); + } + + driver_temp_ma_[driver.name]->Roll(driver.state.temperature); + } // to simplify conditions pass only higher temp of motor drivers + const auto max_element = std::max_element( + driver_temp_ma_.begin(), driver_temp_ma_.end(), + [](const auto & a, const auto & b) { return a.second->GetAverage() < b.second->GetAverage(); }); + safety_tree_manager_->GetBlackboard()->set( - "driver_temp", - std::max({front_driver_temp_ma_->GetAverage(), rear_driver_temp_ma_->GetAverage()})); + "driver_temp", max_element->second->GetAverage()); } void SafetyManagerNode::EStopCB(const BoolMsg::SharedPtr e_stop) @@ -310,4 +295,4 @@ void SafetyManagerNode::ShutdownRobot(const std::string & reason) rclcpp::shutdown(); } -} // namespace panther_manager +} // namespace husarion_ugv_manager diff --git a/panther_manager/src/safety_manager_node_main.cpp b/husarion_ugv_manager/src/safety_manager_node_main.cpp similarity index 86% rename from panther_manager/src/safety_manager_node_main.cpp rename to husarion_ugv_manager/src/safety_manager_node_main.cpp index 3ad34404..68486c86 100644 --- a/panther_manager/src/safety_manager_node_main.cpp +++ b/husarion_ugv_manager/src/safety_manager_node_main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/safety_manager_node.hpp" +#include "husarion_ugv_manager/safety_manager_node.hpp" #include #include @@ -22,7 +22,8 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto safety_manager_node = std::make_shared("safety_manager"); + auto safety_manager_node = + std::make_shared("safety_manager"); safety_manager_node->Initialize(); try { diff --git a/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp similarity index 85% rename from panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp rename to husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp index c9526698..e359bc59 100644 --- a/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp @@ -20,10 +20,10 @@ #include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/plugins/action/call_set_bool_service_node.hpp" +#include "husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp" #include "utils/plugin_test_utils.hpp" -class TestCallSetBoolService : public panther_manager::plugin_test_utils::PluginTestUtils +class TestCallSetBoolService : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils { public: void ServiceCallback( @@ -47,7 +47,7 @@ TEST_F(TestCallSetBoolService, GoodLoadingCallSetBoolServicePlugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - RegisterNodeWithParams("CallSetBoolService"); + RegisterNodeWithParams("CallSetBoolService"); ASSERT_NO_THROW({ CreateTree("CallSetBoolService", service); }); } @@ -56,7 +56,7 @@ TEST_F(TestCallSetBoolService, WrongPluginNameLoadingCallSetBoolServicePlugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - RegisterNodeWithParams("CallSetBoolService"); + RegisterNodeWithParams("CallSetBoolService"); EXPECT_THROW({ CreateTree("WrongCallSetBoolService", service); }, BT::RuntimeError); } @@ -65,7 +65,7 @@ TEST_F(TestCallSetBoolService, WrongCallSetBoolServiceServiceServerNotInitialize { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - RegisterNodeWithParams("CallSetBoolService"); + RegisterNodeWithParams("CallSetBoolService"); CreateTree("CallSetBoolService", service); @@ -86,7 +86,7 @@ TEST_F(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithTrueValue) ServiceCallback(request, response, true, true); }); - RegisterNodeWithParams("CallSetBoolService"); + RegisterNodeWithParams("CallSetBoolService"); CreateTree("CallSetBoolService", service); auto & tree = GetTree(); @@ -106,7 +106,7 @@ TEST_F(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithFalseValue) ServiceCallback(request, response, true, false); }); - RegisterNodeWithParams("CallSetBoolService"); + RegisterNodeWithParams("CallSetBoolService"); CreateTree("CallSetBoolService", service); auto & tree = GetTree(); @@ -126,7 +126,7 @@ TEST_F(TestCallSetBoolService, WrongSetBoolCallServiceFailure) ServiceCallback(request, response, false, false); }); - RegisterNodeWithParams("CallSetBoolService"); + RegisterNodeWithParams("CallSetBoolService"); CreateTree("CallSetBoolService", service); auto & tree = GetTree(); diff --git a/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp similarity index 86% rename from panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp rename to husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp index d44ff4ec..1708ef7d 100644 --- a/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp @@ -20,10 +20,11 @@ #include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/plugins/action/call_set_led_animation_service_node.hpp" +#include "husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp" #include "utils/plugin_test_utils.hpp" -class TestCallSetLedAnimationService : public panther_manager::plugin_test_utils::PluginTestUtils +class TestCallSetLedAnimationService +: public husarion_ugv_manager::plugin_test_utils::PluginTestUtils { public: void ServiceCallback( @@ -54,7 +55,8 @@ TEST_F(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServicePlug std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - RegisterNodeWithParams("CallSetLedAnimationService"); + RegisterNodeWithParams( + "CallSetLedAnimationService"); ASSERT_NO_THROW({ CreateTree("CallSetLedAnimationService", service); }); } @@ -64,7 +66,8 @@ TEST_F(TestCallSetLedAnimationService, WrongPluginNameLoadingCallSetLedAnimation std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - RegisterNodeWithParams("CallSetLedAnimationService"); + RegisterNodeWithParams( + "CallSetLedAnimationService"); EXPECT_THROW({ CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError); } @@ -74,7 +77,8 @@ TEST_F(TestCallSetLedAnimationService, WrongCallSetLedAnimationServiceServiceSer std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - RegisterNodeWithParams("CallSetLedAnimationService"); + RegisterNodeWithParams( + "CallSetLedAnimationService"); CreateTree("CallSetLedAnimationService", service); auto & tree = GetTree(); @@ -88,7 +92,8 @@ TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - RegisterNodeWithParams("CallSetLedAnimationService"); + RegisterNodeWithParams( + "CallSetLedAnimationService"); CreateTree("CallSetLedAnimationService", service); auto & tree = GetTree(); @@ -110,7 +115,8 @@ TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "false"}}; - RegisterNodeWithParams("CallSetLedAnimationService"); + RegisterNodeWithParams( + "CallSetLedAnimationService"); CreateTree("CallSetLedAnimationService", service); auto & tree = GetTree(); @@ -132,7 +138,8 @@ TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith std::map service = { {"service_name", "set_led_animation"}, {"id", "5"}, {"param", ""}, {"repeating", "true"}}; - RegisterNodeWithParams("CallSetLedAnimationService"); + RegisterNodeWithParams( + "CallSetLedAnimationService"); CreateTree("CallSetLedAnimationService", service); auto & tree = GetTree(); @@ -154,7 +161,8 @@ TEST_F(TestCallSetLedAnimationService, WrongSetLedAnimationCallServiceFailure) std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - RegisterNodeWithParams("CallSetLedAnimationService"); + RegisterNodeWithParams( + "CallSetLedAnimationService"); CreateTree("CallSetLedAnimationService", service); auto & tree = GetTree(); diff --git a/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp similarity index 84% rename from panther_manager/test/plugins/action/test_call_trigger_service_node.cpp rename to husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp index e0aa3050..a130e530 100644 --- a/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp @@ -21,10 +21,10 @@ #include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/plugins/action/call_trigger_service_node.hpp" +#include "husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp" #include "utils/plugin_test_utils.hpp" -class TestCallTriggerService : public panther_manager::plugin_test_utils::PluginTestUtils +class TestCallTriggerService : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils { public: void ServiceCallback( @@ -45,7 +45,7 @@ TEST_F(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin) { std::map service = {{"service_name", "trigger"}}; - RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); ASSERT_NO_THROW({ CreateTree("CallTriggerService", service); }); } @@ -54,7 +54,7 @@ TEST_F(TestCallTriggerService, WrongPluginNameLoadingCallTriggerServicePlugin) { std::map service = {{"service_name", "trigger"}}; - RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); EXPECT_THROW({ CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError); } @@ -63,7 +63,7 @@ TEST_F(TestCallTriggerService, WrongCallTriggerServiceServiceServerNotInitialize { std::map service = {{"service_name", "trigger"}}; - RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); CreateTree("CallTriggerService", service); auto & tree = GetTree(); @@ -76,7 +76,7 @@ TEST_F(TestCallTriggerService, GoodTriggerCallServiceSuccess) { std::map service = {{"service_name", "trigger"}}; - RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); CreateTree("CallTriggerService", service); auto & tree = GetTree(); @@ -97,7 +97,7 @@ TEST_F(TestCallTriggerService, WrongTriggerCallServiceFailure) { std::map service = {{"service_name", "trigger"}}; - RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); CreateTree("CallTriggerService", service); auto & tree = GetTree(); diff --git a/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp b/husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp similarity index 77% rename from panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp rename to husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp index edece3f6..980f9e0a 100644 --- a/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp @@ -22,16 +22,16 @@ #include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp" +#include "husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp" #include "utils/plugin_test_utils.hpp" -typedef panther_manager::plugin_test_utils::PluginTestUtils TestShutdownHostsFromFile; +typedef husarion_ugv_manager::plugin_test_utils::PluginTestUtils TestShutdownHostsFromFile; TEST_F(TestShutdownHostsFromFile, GoodLoadingShutdownHostsFromFilePlugin) { const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; - RegisterNodeWithoutParams("ShutdownHostsFromFile"); + RegisterNodeWithoutParams("ShutdownHostsFromFile"); ASSERT_NO_THROW({ CreateTree("ShutdownHostsFromFile", service); }); } @@ -40,7 +40,7 @@ TEST_F(TestShutdownHostsFromFile, WrongPluginNameLoadingShutdownHostsFromFilePlu { const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; - RegisterNodeWithoutParams("ShutdownHostsFromFile"); + RegisterNodeWithoutParams("ShutdownHostsFromFile"); EXPECT_THROW({ CreateTree("WrongShutdownHostsFromFile", service); }, BT::RuntimeError); } @@ -48,10 +48,11 @@ TEST_F(TestShutdownHostsFromFile, WrongPluginNameLoadingShutdownHostsFromFilePlu TEST_F(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile) { const std::string file_path = - testing::TempDir() + "test_panther_manager_wrong_cannot_find_file_shutdown_hosts_from_file"; + testing::TempDir() + + "test_husarion_ugv_manager_wrong_cannot_find_file_shutdown_hosts_from_file"; const std::map service = {{"shutdown_hosts_file", file_path}}; - RegisterNodeWithoutParams("ShutdownHostsFromFile"); + RegisterNodeWithoutParams("ShutdownHostsFromFile"); CreateTree("ShutdownHostsFromFile", service); auto & tree = GetTree(); @@ -62,10 +63,10 @@ TEST_F(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile) TEST_F(TestShutdownHostsFromFile, GoodShutdownHostsFromFile) { - const std::string config_file_path = testing::TempDir() + - "test_panther_manager_good_shutdown_hosts_from_file_config"; + const std::string config_file_path = + testing::TempDir() + "test_husarion_ugv_manager_good_shutdown_hosts_from_file_config"; const std::string test_file_path = testing::TempDir() + - "test_panther_manager_good_shutdown_hosts_from_file"; + "test_husarion_ugv_manager_good_shutdown_hosts_from_file"; std::filesystem::remove(test_file_path); std::filesystem::remove(config_file_path); @@ -89,7 +90,7 @@ TEST_F(TestShutdownHostsFromFile, GoodShutdownHostsFromFile) config_file.close(); const std::map service = {{"shutdown_hosts_file", config_file_path}}; - RegisterNodeWithoutParams("ShutdownHostsFromFile"); + RegisterNodeWithoutParams("ShutdownHostsFromFile"); auto & tree = GetTree(); CreateTree("ShutdownHostsFromFile", service); diff --git a/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp b/husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp similarity index 81% rename from panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp rename to husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp index b174b7dc..5ed922dc 100644 --- a/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp @@ -22,10 +22,10 @@ #include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/plugins/action/shutdown_single_host_node.hpp" +#include "husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp" #include "utils/plugin_test_utils.hpp" -typedef panther_manager::plugin_test_utils::PluginTestUtils TestShutdownSingleHost; +typedef husarion_ugv_manager::plugin_test_utils::PluginTestUtils TestShutdownSingleHost; TEST_F(TestShutdownSingleHost, GoodLoadingShutdownSingleHostPlugin) { @@ -34,7 +34,7 @@ TEST_F(TestShutdownSingleHost, GoodLoadingShutdownSingleHostPlugin) {"port", "22"}, {"timeout", "5.0"}, {"username", "husarion"}, }; - RegisterNodeWithoutParams("ShutdownSingleHost"); + RegisterNodeWithoutParams("ShutdownSingleHost"); ASSERT_NO_THROW({ CreateTree("ShutdownSingleHost", service); }); } @@ -46,13 +46,13 @@ TEST_F(TestShutdownSingleHost, WrongPluginNameLoadingShutdownSingleHostPlugin) {"port", "22"}, {"timeout", "5.0"}, {"username", "husarion"}, }; - RegisterNodeWithoutParams("ShutdownSingleHost"); + RegisterNodeWithoutParams("ShutdownSingleHost"); EXPECT_THROW({ CreateTree("WrongShutdownSingleHost", service); }, BT::RuntimeError); } TEST_F(TestShutdownSingleHost, GoodTouchCommand) { - std::string test_file_path = testing::TempDir() + "test_panther_manager_good_touch_command"; + std::string test_file_path = testing::TempDir() + "test_husarion_ugv_manager_good_touch_command"; std::filesystem::remove(test_file_path); EXPECT_FALSE(std::filesystem::exists(test_file_path)); std::map service = { @@ -63,7 +63,7 @@ TEST_F(TestShutdownSingleHost, GoodTouchCommand) {"timeout", "5.0"}, {"username", "husarion"}, }; - RegisterNodeWithoutParams("ShutdownSingleHost"); + RegisterNodeWithoutParams("ShutdownSingleHost"); CreateTree("ShutdownSingleHost", service); auto & tree = GetTree(); @@ -80,7 +80,7 @@ TEST_F(TestShutdownSingleHost, Timeout) {"command", "sleep 0.5"}, {"ip", "localhost"}, {"ping_for_success", "false"}, {"port", "22"}, {"timeout", "0.1"}, {"username", "husarion"}, }; - RegisterNodeWithoutParams("ShutdownSingleHost"); + RegisterNodeWithoutParams("ShutdownSingleHost"); CreateTree("ShutdownSingleHost", service); auto & tree = GetTree(); @@ -98,7 +98,7 @@ TEST_F(TestShutdownSingleHost, WrongUser) {"timeout", "0.2"}, {"username", "wrong_user"}, }; - RegisterNodeWithoutParams("ShutdownSingleHost"); + RegisterNodeWithoutParams("ShutdownSingleHost"); CreateTree("ShutdownSingleHost", service); auto & tree = GetTree(); diff --git a/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp b/husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp similarity index 85% rename from panther_manager/test/plugins/action/test_signal_shutdown_node.cpp rename to husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp index 9fd109c2..3b0a383b 100644 --- a/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp @@ -21,16 +21,16 @@ #include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/plugins/action/signal_shutdown_node.hpp" +#include "husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp" #include "utils/plugin_test_utils.hpp" -typedef panther_manager::plugin_test_utils::PluginTestUtils TestSignalShutdown; +typedef husarion_ugv_manager::plugin_test_utils::PluginTestUtils TestSignalShutdown; TEST_F(TestSignalShutdown, GoodLoadingSignalShutdownPlugin) { std::map service = {{"reason", "Test shutdown."}}; - RegisterNodeWithoutParams("SignalShutdown"); + RegisterNodeWithoutParams("SignalShutdown"); ASSERT_NO_THROW({ CreateTree("SignalShutdown", service); }); } @@ -46,7 +46,7 @@ TEST_F(TestSignalShutdown, GoodCheckReasonBlackboardValue) { std::map service = {{"reason", "Test shutdown."}}; - RegisterNodeWithoutParams("SignalShutdown"); + RegisterNodeWithoutParams("SignalShutdown"); CreateTree("SignalShutdown", service); auto & tree = GetTree(); diff --git a/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp b/husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp similarity index 89% rename from panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp rename to husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp index deba7db9..fb2eca46 100644 --- a/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp +++ b/husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp @@ -21,10 +21,10 @@ #include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/plugins/decorator/tick_after_timeout_node.hpp" +#include "husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp" #include "utils/plugin_test_utils.hpp" -class TestTickAfterTimeout : public panther_manager::plugin_test_utils::PluginTestUtils +class TestTickAfterTimeout : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils { public: virtual std::string BuildBehaviorTree( @@ -51,7 +51,7 @@ std::string TestTickAfterTimeout::BuildBehaviorTree( TEST_F(TestTickAfterTimeout, GoodTickAfterTimeout) { - RegisterNodeWithoutParams("TickAfterTimeout"); + RegisterNodeWithoutParams("TickAfterTimeout"); CreateTree("", {}); auto & tree = GetTree(); diff --git a/panther_manager/test/plugins/test_shutdown_host.cpp b/husarion_ugv_manager/test/plugins/test_shutdown_host.cpp similarity index 64% rename from panther_manager/test/plugins/test_shutdown_host.cpp rename to husarion_ugv_manager/test/plugins/test_shutdown_host.cpp index 25317ee5..cd48e27d 100644 --- a/panther_manager/test/plugins/test_shutdown_host.cpp +++ b/husarion_ugv_manager/test/plugins/test_shutdown_host.cpp @@ -16,7 +16,7 @@ #include "gtest/gtest.h" -#include "panther_manager/plugins/shutdown_host.hpp" +#include "husarion_ugv_manager/plugins/shutdown_host.hpp" class TestShutdownHost : public testing::Test { @@ -26,18 +26,18 @@ class TestShutdownHost : public testing::Test const float timeout, const bool ping_for_success); bool IsAvailable(); void Call(); - panther_manager::ShutdownHostState GetState() const; + husarion_ugv_manager::ShutdownHostState GetState() const; std::string GetResponse() const; private: - std::unique_ptr shutdown_host; + std::unique_ptr shutdown_host; }; void TestShutdownHost::Create( const std::string ip, const std::string user, const int port, const std::string command, const float timeout, const bool ping_for_success) { - shutdown_host = std::make_unique( + shutdown_host = std::make_unique( ip, user, port, command, timeout, ping_for_success); } @@ -45,7 +45,7 @@ bool TestShutdownHost::IsAvailable() { return shutdown_host->IsAvailable(); } void TestShutdownHost::Call() { shutdown_host->Call(); } -panther_manager::ShutdownHostState TestShutdownHost::GetState() const +husarion_ugv_manager::ShutdownHostState TestShutdownHost::GetState() const { return shutdown_host->GetState(); } @@ -56,7 +56,7 @@ TEST_F(TestShutdownHost, GoodCheckIsAvailable) { Create("127.0.0.1", "husarion", 22, "echo HelloWorld", 0.1, true); EXPECT_TRUE(IsAvailable()); - EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE); + EXPECT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::IDLE); } TEST_F(TestShutdownHost, WrongCheckIsAvailable) @@ -70,32 +70,32 @@ TEST_F(TestShutdownHost, GoodCommandExecute) Create("127.0.0.1", "husarion", 22, "echo HelloWorld", 0.1, false); ASSERT_TRUE(IsAvailable()); - ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE); + ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::IDLE); Call(); - ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::COMMAND_EXECUTED); + ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::COMMAND_EXECUTED); // Wait for response - while (GetState() == panther_manager::ShutdownHostState::COMMAND_EXECUTED) { + while (GetState() == husarion_ugv_manager::ShutdownHostState::COMMAND_EXECUTED) { Call(); } - ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::RESPONSE_RECEIVED); + ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::RESPONSE_RECEIVED); const auto response = GetResponse(); EXPECT_EQ(response, "HelloWorld\n"); Call(); - ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::PINGING); + ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::PINGING); Call(); - ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::SUCCESS); + ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::SUCCESS); } TEST_F(TestShutdownHost, WrongHostPing) { Create("1.45.23.26", "husarion", 22, "echo HelloWorld", 0.1, true); - EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE); + EXPECT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::IDLE); Call(); - EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::SKIPPED); + EXPECT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::SKIPPED); } TEST_F(TestShutdownHost, CheckTimeout) @@ -103,14 +103,14 @@ TEST_F(TestShutdownHost, CheckTimeout) Create("127.0.0.1", "husarion", 22, "sleep 0.2", 0.1, false); ASSERT_TRUE(IsAvailable()); - ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE); + ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::IDLE); Call(); - ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::COMMAND_EXECUTED); + ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::COMMAND_EXECUTED); // Wait for response - while (GetState() == panther_manager::ShutdownHostState::COMMAND_EXECUTED) { + while (GetState() == husarion_ugv_manager::ShutdownHostState::COMMAND_EXECUTED) { Call(); } - ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::FAILURE); + ASSERT_EQ(GetState(), husarion_ugv_manager::ShutdownHostState::FAILURE); } int main(int argc, char ** argv) diff --git a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp b/husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp similarity index 63% rename from panther_manager/test/plugins/test_shutdown_hosts_node.cpp rename to husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp index 1395f439..eb797110 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp +++ b/husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp @@ -16,24 +16,26 @@ #include "gtest/gtest.h" -#include "panther_manager/plugins/shutdown_hosts_node.hpp" +#include "husarion_ugv_manager/plugins/shutdown_hosts_node.hpp" -class ShutdownHostsNodeWrapper : public panther_manager::ShutdownHosts +class ShutdownHostsNodeWrapper : public husarion_ugv_manager::ShutdownHosts { public: ShutdownHostsNodeWrapper(const std::string & name, const BT::NodeConfig & conf) - : panther_manager::ShutdownHosts(name, conf) + : husarion_ugv_manager::ShutdownHosts(name, conf) { } - void RemoveDuplicatedHosts(std::vector> & hosts); - std::vector> & GetHosts(); + void RemoveDuplicatedHosts( + std::vector> & hosts); + std::vector> & GetHosts(); BT::NodeStatus onRunning(); BT::NodeStatus onStart(); virtual bool UpdateHosts( - std::vector> & hosts) override final; + std::vector> & hosts) override final; void SetHostsAndSuccess( - std::vector> hosts, const bool returned_status); + std::vector> hosts, + const bool returned_status); static BT::PortsList providedPorts() { @@ -43,40 +45,42 @@ class ShutdownHostsNodeWrapper : public panther_manager::ShutdownHosts } private: - std::vector> hosts_to_set; + std::vector> hosts_to_set; bool update_hosts_success_ = true; }; void ShutdownHostsNodeWrapper::RemoveDuplicatedHosts( - std::vector> & hosts) + std::vector> & hosts) { - panther_manager::ShutdownHosts::RemoveDuplicatedHosts(hosts); + husarion_ugv_manager::ShutdownHosts::RemoveDuplicatedHosts(hosts); } -std::vector> & ShutdownHostsNodeWrapper::GetHosts() +std::vector> & +ShutdownHostsNodeWrapper::GetHosts() { return hosts_; } BT::NodeStatus ShutdownHostsNodeWrapper::onRunning() { - return panther_manager::ShutdownHosts::onRunning(); + return husarion_ugv_manager::ShutdownHosts::onRunning(); } BT::NodeStatus ShutdownHostsNodeWrapper::onStart() { - return panther_manager::ShutdownHosts::onStart(); + return husarion_ugv_manager::ShutdownHosts::onStart(); } bool ShutdownHostsNodeWrapper::UpdateHosts( - std::vector> & hosts) + std::vector> & hosts) { hosts = hosts_to_set; return update_hosts_success_; } void ShutdownHostsNodeWrapper::SetHostsAndSuccess( - std::vector> hosts, const bool returned_status) + std::vector> hosts, + const bool returned_status) { hosts_to_set = hosts; update_hosts_success_ = returned_status; @@ -86,14 +90,14 @@ class ShutdownHostsNodeTest : public testing::Test { public: void CreateWrapper( - std::vector> hosts, const bool success); + std::vector> hosts, const bool success); protected: std::unique_ptr wrapper; }; void ShutdownHostsNodeTest::CreateWrapper( - std::vector> hosts, const bool success) + std::vector> hosts, const bool success) { BT::NodeConfig conf; wrapper = std::make_unique("Duplicated hosts", conf); @@ -103,14 +107,14 @@ void ShutdownHostsNodeTest::CreateWrapper( TEST_F(ShutdownHostsNodeTest, GoodRemovingDuplicatedHosts) { CreateWrapper( - {std::make_shared( + {std::make_shared( "127.0.0.1", "husarion", 22, "echo HelloWorld", 1.0, true), - std::make_shared( + std::make_shared( "localhost", "husarion", 22, "echo HelloWorld", 1.0, true), - std::make_shared( + std::make_shared( "localhost", "husarion", 22, "echo HelloWorld", 1.0, true)}, true); - std::vector> hosts; + std::vector> hosts; ASSERT_TRUE(wrapper->UpdateHosts(hosts)); ASSERT_EQ(hosts.size(), 3); wrapper->RemoveDuplicatedHosts(hosts); @@ -120,7 +124,7 @@ TEST_F(ShutdownHostsNodeTest, GoodRemovingDuplicatedHosts) TEST_F(ShutdownHostsNodeTest, FailedWhenUpdateHostReturnsFalse) { CreateWrapper( - {std::make_shared( + {std::make_shared( "127.0.0.1", "husarion", 22, "echo HelloWorld", 1.0, true)}, false); @@ -139,7 +143,7 @@ TEST_F(ShutdownHostsNodeTest, FailedWhenHostsAreEmpty) TEST_F(ShutdownHostsNodeTest, CheckFailedHosts) { CreateWrapper( - {std::make_shared( + {std::make_shared( "127.0.0.1", "husarion", 22, "echo HelloWorld", 1.0, true)}, true); auto status = wrapper->onStart(); diff --git a/panther_manager/test/test_behavior_tree_manager.cpp b/husarion_ugv_manager/test/test_behavior_tree_manager.cpp similarity index 91% rename from panther_manager/test/test_behavior_tree_manager.cpp rename to husarion_ugv_manager/test/test_behavior_tree_manager.cpp index ca6925e2..80807d0e 100644 --- a/panther_manager/test/test_behavior_tree_manager.cpp +++ b/husarion_ugv_manager/test/test_behavior_tree_manager.cpp @@ -25,10 +25,10 @@ #include "behaviortree_cpp/tree_node.h" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/behavior_tree_manager.hpp" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_manager.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" -class BehaviorTreeManagerWrapper : public panther_manager::BehaviorTreeManager +class BehaviorTreeManagerWrapper : public husarion_ugv_manager::BehaviorTreeManager { public: BehaviorTreeManagerWrapper( @@ -69,7 +69,7 @@ TEST_F(TestBehaviorTreeManager, CreateBTConfigInvalidItem) { const std::map bb_values = {{"value", 1l}}; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { behavior_tree_manager_->CreateBTConfig(bb_values); }, "Invalid type for blackboard entry.")); } @@ -109,7 +109,7 @@ TEST_F(TestBehaviorTreeManager, InitializeInvalidTreeName) )"; ASSERT_NO_THROW(factory_.registerBehaviorTreeFromText(tree_xml)); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { behavior_tree_manager_->Initialize(factory_); }, "Can't find a tree with name: " + std::string(kTreeName))); } diff --git a/panther_manager/test/test_behavior_tree_utils.cpp b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp similarity index 90% rename from panther_manager/test/test_behavior_tree_utils.cpp rename to husarion_ugv_manager/test/test_behavior_tree_utils.cpp index 75b0cb0a..ac32b59e 100644 --- a/panther_manager/test/test_behavior_tree_utils.cpp +++ b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp @@ -26,8 +26,8 @@ #include "behaviortree_ros2/ros_node_params.hpp" #include "rclcpp/rclcpp.hpp" -#include "panther_manager/behavior_tree_utils.hpp" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" class TestRegisterBT : public testing::Test { @@ -74,7 +74,7 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeInvalidPlugin) BT::BehaviorTreeFactory factory; EXPECT_THROW( - panther_manager::behavior_tree_utils::RegisterBehaviorTree( + husarion_ugv_manager::behavior_tree_utils::RegisterBehaviorTree( factory, bt_project_path_, {"invalid_bt_node"}), BT::RuntimeError); } @@ -83,7 +83,7 @@ TEST_F(TestRegisterBT, RegisterBehaviorTree) { BT::BehaviorTreeFactory factory; - EXPECT_NO_THROW(panther_manager::behavior_tree_utils::RegisterBehaviorTree( + EXPECT_NO_THROW(husarion_ugv_manager::behavior_tree_utils::RegisterBehaviorTree( factory, bt_project_path_, {"tick_after_timeout_bt_node", "signal_shutdown_bt_node"})); // check if nodes were registered @@ -105,7 +105,7 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeROS) BT::RosNodeParams params; params.nh = std::make_shared("test_node"); - EXPECT_NO_THROW(panther_manager::behavior_tree_utils::RegisterBehaviorTree( + EXPECT_NO_THROW(husarion_ugv_manager::behavior_tree_utils::RegisterBehaviorTree( factory, bt_project_path_, {}, params, {"call_trigger_service_bt_node", "call_set_bool_service_bt_node"})); diff --git a/panther_manager/test/test_lights_behavior_tree.cpp b/husarion_ugv_manager/test/test_lights_behavior_tree.cpp similarity index 92% rename from panther_manager/test/test_lights_behavior_tree.cpp rename to husarion_ugv_manager/test/test_lights_behavior_tree.cpp index e96bce13..e44285a6 100644 --- a/panther_manager/test/test_lights_behavior_tree.cpp +++ b/husarion_ugv_manager/test/test_lights_behavior_tree.cpp @@ -22,6 +22,7 @@ #include "gtest/gtest.h" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "behaviortree_cpp/basic_types.h" #include "rclcpp/rclcpp.hpp" @@ -31,7 +32,7 @@ #include "panther_msgs/msg/led_animation.hpp" #include "panther_msgs/srv/set_led_animation.hpp" -#include +#include #include using BoolMsg = std_msgs::msg::Bool; @@ -39,7 +40,7 @@ using BatteryStateMsg = sensor_msgs::msg::BatteryState; using SetLEDAnimationSrv = panther_msgs::srv::SetLEDAnimation; using LEDAnimationMsg = panther_msgs::msg::LEDAnimation; -class LightsManagerNodeWrapper : public panther_manager::LightsManagerNode +class LightsManagerNodeWrapper : public husarion_ugv_manager::LightsManagerNode { public: LightsManagerNodeWrapper( @@ -107,6 +108,10 @@ TestLightsBehaviorTree::TestLightsBehaviorTree() std::vector TestLightsBehaviorTree::CreateTestParameters() const { + const auto panther_manager_pkg_path = + ament_index_cpp::get_package_share_directory("husarion_ugv_manager"); + const std::string bt_project_path = panther_manager_pkg_path + "/behavior_trees/LightsBT.btproj"; + std::vector plugin_libs; plugin_libs.push_back("tick_after_timeout_bt_node"); @@ -114,11 +119,11 @@ std::vector TestLightsBehaviorTree::CreateTestParameters() co ros_plugin_libs.push_back("call_set_led_animation_service_bt_node"); std::vector params; + params.push_back(rclcpp::Parameter("bt_project_path", bt_project_path)); params.push_back(rclcpp::Parameter("plugin_libs", plugin_libs)); params.push_back(rclcpp::Parameter("ros_plugin_libs", ros_plugin_libs)); - params.push_back(rclcpp::Parameter("battery.animation_period.low", kLowBatteryAnimPeriod)); - params.push_back( - rclcpp::Parameter("battery.animation_period.critical", kCriticalBatteryAnimPeriod)); + params.push_back(rclcpp::Parameter("battery.anim_period.low", kLowBatteryAnimPeriod)); + params.push_back(rclcpp::Parameter("battery.anim_period.critical", kCriticalBatteryAnimPeriod)); return params; } diff --git a/panther_manager/test/test_lights_manager_node.cpp b/husarion_ugv_manager/test/test_lights_manager_node.cpp similarity index 94% rename from panther_manager/test/test_lights_manager_node.cpp rename to husarion_ugv_manager/test/test_lights_manager_node.cpp index ea60e529..cb3ef5b0 100644 --- a/panther_manager/test/test_lights_manager_node.cpp +++ b/husarion_ugv_manager/test/test_lights_manager_node.cpp @@ -29,13 +29,13 @@ #include "sensor_msgs/msg/battery_state.hpp" #include "std_msgs/msg/bool.hpp" -#include -#include "panther_utils/test/ros_test_utils.hpp" +#include +#include "husarion_ugv_utils/test/ros_test_utils.hpp" using BoolMsg = std_msgs::msg::Bool; using BatteryStateMsg = sensor_msgs::msg::BatteryState; -class LightsManagerNodeWrapper : public panther_manager::LightsManagerNode +class LightsManagerNodeWrapper : public husarion_ugv_manager::LightsManagerNode { public: LightsManagerNodeWrapper( @@ -146,7 +146,7 @@ TEST_F(TestLightsManagerNode, BatteryCBBlackboardUpdate) battery_state.power_supply_status = expected_status; battery_state.power_supply_health = expected_health; - panther_utils::test_utils::PublishAndSpin( + husarion_ugv_utils::test_utils::PublishAndSpin( lights_manager_node_, "battery/battery_status", battery_state); auto blackboard = lights_manager_node_->GetLightsTreeBlackboard(); @@ -162,7 +162,7 @@ TEST_F(TestLightsManagerNode, EStopCBBlackboardUpdate) auto bool_msg = std_msgs::msg::Bool(); bool_msg.data = expected_state; - panther_utils::test_utils::PublishAndSpin( + husarion_ugv_utils::test_utils::PublishAndSpin( lights_manager_node_, "hardware/e_stop", bool_msg, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); diff --git a/panther_manager/test/test_safety_behavior_tree.cpp b/husarion_ugv_manager/test/test_safety_behavior_tree.cpp similarity index 96% rename from panther_manager/test/test_safety_behavior_tree.cpp rename to husarion_ugv_manager/test/test_safety_behavior_tree.cpp index ab7d978a..9e7dc51d 100644 --- a/panther_manager/test/test_safety_behavior_tree.cpp +++ b/husarion_ugv_manager/test/test_safety_behavior_tree.cpp @@ -22,6 +22,7 @@ #include "gtest/gtest.h" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "behaviortree_cpp/basic_types.h" #include "rclcpp/rclcpp.hpp" @@ -30,22 +31,20 @@ #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" -#include "panther_msgs/msg/driver_state.hpp" #include "panther_msgs/msg/io_state.hpp" #include "panther_msgs/msg/system_status.hpp" -#include +#include #include using BoolMsg = std_msgs::msg::Bool; using BatteryStateMsg = sensor_msgs::msg::BatteryState; using IOStateMsg = panther_msgs::msg::IOState; -using DriverStateMsg = panther_msgs::msg::DriverState; using SystemStatusMsg = panther_msgs::msg::SystemStatus; using SetBoolSrv = std_srvs::srv::SetBool; using TriggerSrv = std_srvs::srv::Trigger; -class SafetyManagerNodeWrapper : public panther_manager::SafetyManagerNode +class SafetyManagerNodeWrapper : public husarion_ugv_manager::SafetyManagerNode { public: SafetyManagerNodeWrapper( @@ -128,6 +127,10 @@ TestSafetyBehaviorTree::~TestSafetyBehaviorTree() { rclcpp::shutdown(); } std::vector TestSafetyBehaviorTree::CreateTestParameters() const { + const auto panther_manager_pkg_path = + ament_index_cpp::get_package_share_directory("husarion_ugv_manager"); + const std::string bt_project_path = panther_manager_pkg_path + "/behavior_trees/SafetyBT.btproj"; + std::vector plugin_libs; plugin_libs.push_back("tick_after_timeout_bt_node"); plugin_libs.push_back("shutdown_single_host_bt_node"); @@ -139,6 +142,7 @@ std::vector TestSafetyBehaviorTree::CreateTestParameters() co ros_plugin_libs.push_back("call_trigger_service_bt_node"); std::vector params; + params.push_back(rclcpp::Parameter("bt_project_path", bt_project_path)); params.push_back(rclcpp::Parameter("plugin_libs", plugin_libs)); params.push_back(rclcpp::Parameter("ros_plugin_libs", ros_plugin_libs)); params.push_back(rclcpp::Parameter("fan_turn_off_timeout", kFanTurnOffTimeout)); diff --git a/panther_manager/test/test_safety_manager_node.cpp b/husarion_ugv_manager/test/test_safety_manager_node.cpp similarity index 90% rename from panther_manager/test/test_safety_manager_node.cpp rename to husarion_ugv_manager/test/test_safety_manager_node.cpp index f7db1354..036ccdfe 100644 --- a/panther_manager/test/test_safety_manager_node.cpp +++ b/husarion_ugv_manager/test/test_safety_manager_node.cpp @@ -29,13 +29,13 @@ #include "sensor_msgs/msg/battery_state.hpp" #include "std_msgs/msg/bool.hpp" -#include -#include "panther_utils/test/ros_test_utils.hpp" +#include +#include "husarion_ugv_utils/test/ros_test_utils.hpp" using BoolMsg = std_msgs::msg::Bool; using BatteryStateMsg = sensor_msgs::msg::BatteryState; -class SafetyManagerNodeWrapper : public panther_manager::SafetyManagerNode +class SafetyManagerNodeWrapper : public husarion_ugv_manager::SafetyManagerNode { public: SafetyManagerNodeWrapper( @@ -167,7 +167,7 @@ TEST_F(TestSafetyManagerNode, BatteryCBBlackboardUpdate) battery_state.power_supply_health = expected_health; battery_state.temperature = expected_temp; - panther_utils::test_utils::PublishAndSpin( + husarion_ugv_utils::test_utils::PublishAndSpin( safety_manager_node_, "battery/battery_status", battery_state); auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard(); @@ -183,7 +183,7 @@ TEST_F(TestSafetyManagerNode, EStopCBBlackboardUpdate) auto bool_msg = std_msgs::msg::Bool(); bool_msg.data = expected_state; - panther_utils::test_utils::PublishAndSpin( + husarion_ugv_utils::test_utils::PublishAndSpin( safety_manager_node_, "hardware/e_stop", bool_msg, transient_local_qos); auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard(); @@ -194,12 +194,14 @@ TEST_F(TestSafetyManagerNode, DriverStateCBBlackboardUpdate) { const float expected_temp = 21.0; - auto driver_state_msg = panther_msgs::msg::DriverState(); - driver_state_msg.front.temperature = expected_temp; - driver_state_msg.rear.temperature = expected_temp; + panther_msgs::msg::DriverStateNamed driver_state; + driver_state.state.temperature = expected_temp; - panther_utils::test_utils::PublishAndSpin( - safety_manager_node_, "hardware/motor_controllers_state", driver_state_msg); + auto driver_state_msg = panther_msgs::msg::RobotDriverState(); + driver_state_msg.driver_states.push_back(driver_state); + + husarion_ugv_utils::test_utils::PublishAndSpin( + safety_manager_node_, "hardware/robot_driver_state", driver_state_msg); auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard(); EXPECT_FLOAT_EQ(blackboard->get("driver_temp"), expected_temp); @@ -214,7 +216,7 @@ TEST_F(TestSafetyManagerNode, IOStateCBBlackboardUpdate) io_state_msg.aux_power = expected_aux_state; io_state_msg.fan = expected_fan_state; - panther_utils::test_utils::PublishAndSpin( + husarion_ugv_utils::test_utils::PublishAndSpin( safety_manager_node_, "hardware/io_state", io_state_msg, transient_local_qos); auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard(); @@ -229,7 +231,7 @@ TEST_F(TestSafetyManagerNode, SystemStatusCBBlackboardUpdate) auto system_status_msg = panther_msgs::msg::SystemStatus(); system_status_msg.cpu_temp = expected_temp; - panther_utils::test_utils::PublishAndSpin( + husarion_ugv_utils::test_utils::PublishAndSpin( safety_manager_node_, "system_status", system_status_msg); auto blackboard = safety_manager_node_->GetSafetyTreeBlackboard(); diff --git a/panther_manager/test/utils/behavior_tree_test_utils.hpp b/husarion_ugv_manager/test/utils/behavior_tree_test_utils.hpp similarity index 88% rename from panther_manager/test/utils/behavior_tree_test_utils.hpp rename to husarion_ugv_manager/test/utils/behavior_tree_test_utils.hpp index 2a739811..05162ce8 100644 --- a/panther_manager/test/utils/behavior_tree_test_utils.hpp +++ b/husarion_ugv_manager/test/utils/behavior_tree_test_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_ -#define PANTHER_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_ +#ifndef HUSARION_UGV_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_ +#define HUSARION_UGV_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_ #include #include @@ -57,4 +57,4 @@ bool SpinWhileRunning( } // namespace behavior_tree::test_utils -#endif // PANTHER_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_ +#endif // HUSARION_UGV_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_ diff --git a/panther_manager/test/utils/plugin_test_utils.hpp b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp similarity index 80% rename from panther_manager/test/utils/plugin_test_utils.hpp rename to husarion_ugv_manager/test/utils/plugin_test_utils.hpp index 16a4a605..f9d4af87 100644 --- a/panther_manager/test/utils/plugin_test_utils.hpp +++ b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ -#define PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGIN_TEST_UTILS_HPP_ +#define HUSARION_UGV_MANAGER_PLUGIN_TEST_UTILS_HPP_ #include #include @@ -32,15 +32,15 @@ #include "panther_msgs/srv/set_led_animation.hpp" -#include "panther_manager/plugins/action/call_set_bool_service_node.hpp" -#include "panther_manager/plugins/action/call_set_led_animation_service_node.hpp" -#include "panther_manager/plugins/action/call_trigger_service_node.hpp" -#include "panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp" -#include "panther_manager/plugins/action/shutdown_single_host_node.hpp" -#include "panther_manager/plugins/action/signal_shutdown_node.hpp" -#include "panther_manager/plugins/decorator/tick_after_timeout_node.hpp" +#include "husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp" +#include "husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp" +#include "husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp" +#include "husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp" +#include "husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp" +#include "husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp" +#include "husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp" -namespace panther_manager::plugin_test_utils +namespace husarion_ugv_manager::plugin_test_utils { struct BehaviorTreePluginDescription @@ -55,7 +55,7 @@ class PluginTestUtils : public testing::Test PluginTestUtils() { rclcpp::init(0, nullptr); - bt_node_ = std::make_shared("test_panther_manager_node"); + bt_node_ = std::make_shared("test_husarion_ugv_manager_node"); } ~PluginTestUtils() @@ -153,5 +153,5 @@ class PluginTestUtils : public testing::Test )"; }; -} // namespace panther_manager::plugin_test_utils -#endif // PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ +} // namespace husarion_ugv_manager::plugin_test_utils +#endif // HUSARION_UGV_MANAGER_PLUGIN_TEST_UTILS_HPP_ diff --git a/panther_utils/CHANGELOG.rst b/husarion_ugv_utils/CHANGELOG.rst similarity index 100% rename from panther_utils/CHANGELOG.rst rename to husarion_ugv_utils/CHANGELOG.rst diff --git a/panther_utils/CMakeLists.txt b/husarion_ugv_utils/CMakeLists.txt similarity index 99% rename from panther_utils/CMakeLists.txt rename to husarion_ugv_utils/CMakeLists.txt index 9fbca544..6753ec7c 100644 --- a/panther_utils/CMakeLists.txt +++ b/husarion_ugv_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(panther_utils) +project(husarion_ugv_utils) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/panther_utils/README.md b/husarion_ugv_utils/README.md similarity index 52% rename from panther_utils/README.md rename to husarion_ugv_utils/README.md index 78fbdf34..62ae9140 100644 --- a/panther_utils/README.md +++ b/husarion_ugv_utils/README.md @@ -1,3 +1,3 @@ -# panther_utils +# husarion_ugv_utils -Package containing commonly used functions, classes, and configurations for the Husarion Panther system. +Package containing commonly used functions, classes, and configurations for the Husarion UGV system. diff --git a/panther_utils/panther_utils/__init__.py b/husarion_ugv_utils/husarion_ugv_utils/__init__.py similarity index 100% rename from panther_utils/panther_utils/__init__.py rename to husarion_ugv_utils/husarion_ugv_utils/__init__.py diff --git a/panther_utils/panther_utils/integration_test_utils.py b/husarion_ugv_utils/husarion_ugv_utils/integration_test_utils.py similarity index 100% rename from panther_utils/panther_utils/integration_test_utils.py rename to husarion_ugv_utils/husarion_ugv_utils/integration_test_utils.py diff --git a/panther_utils/panther_utils/welcomeMsg.py b/husarion_ugv_utils/husarion_ugv_utils/messages.py similarity index 69% rename from panther_utils/panther_utils/welcomeMsg.py rename to husarion_ugv_utils/husarion_ugv_utils/messages.py index ae200ccd..560f1d82 100644 --- a/panther_utils/panther_utils/welcomeMsg.py +++ b/husarion_ugv_utils/husarion_ugv_utils/messages.py @@ -20,10 +20,33 @@ import click from launch.actions import LogInfo from launch.some_substitutions_type import SomeSubstitutionsType -from launch.substitutions import Command +from launch.substitutions import Command, PythonExpression + +LYNX_ASCII = r""" + _ + | | _ _ _ __ __ __ + | | | | | | '_ \\ \/ / + | |__| |_| | | | |> < + |_____\__, |_| |_/_/\_\ + |___/ + + """ # noqa: W605 + +PANTHER_ASCII = r""" + ____ _ _ + | _ \ __ _ _ __ | |_| |__ ___ _ __ + | |_) / _` | '_ \| __| '_ \ / _ \ '__| + | __/ (_| | | | | |_| | | | __/ | + |_| \__,_|_| |_|\__|_| |_|\___|_| + + """ # noqa: W605 + +LYNX_TEXT = click.style(textwrap.dedent(LYNX_ASCII), bold=True) +PANTHER_TEXT = click.style(textwrap.dedent(PANTHER_ASCII), bold=True) def flatten(lst): + """Flatten a nested list into a single list.""" if isinstance(lst, list): flat_list = [] for element in lst: @@ -33,23 +56,18 @@ def flatten(lst): return [lst] -def welcomeMsg( +def welcome_msg( + robot_model: SomeSubstitutionsType, serial_number: SomeSubstitutionsType, robot_hw_version: SomeSubstitutionsType, additional_stats: Dict = {}, ): - pkg_version = Command(command="ros2 pkg xml -t version panther") + """Generate a welcome message with robot information and stats.""" + pkg_version = Command(command="ros2 pkg xml -t version husarion_ugv") - PANTHER_TEXT = """ - ____ _ _ - | _ \ __ _ _ __ | |_| |__ ___ _ __ - | |_) / _` | '_ \| __| '_ \ / _ \ '__| - | __/ (_| | | | | |_| | | | __/ | - |_| \__,_|_| |_|\__|_| |_|\___|_| - - """ # noqa: W605 - pth_txt = textwrap.dedent(PANTHER_TEXT) - pth_txt = click.style(pth_txt, bold=True) + robot_model_expr = PythonExpression( + [f"r'''{LYNX_TEXT}''' if '", robot_model, f"' == 'lynx' else r'''{PANTHER_TEXT}'''"] + ) stats_to_show = { "Serial Number": serial_number, @@ -68,6 +86,6 @@ def welcomeMsg( ] stats_msg = flatten(nested_list_of_stats) - stats_msg.insert(0, pth_txt) + stats_msg.insert(0, robot_model_expr) return LogInfo(msg=stats_msg) diff --git a/panther_utils/include/panther_utils/common_utilities.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/common_utilities.hpp similarity index 78% rename from panther_utils/include/panther_utils/common_utilities.hpp rename to husarion_ugv_utils/include/husarion_ugv_utils/common_utilities.hpp index a8022559..a82b0927 100644 --- a/panther_utils/include/panther_utils/common_utilities.hpp +++ b/husarion_ugv_utils/include/husarion_ugv_utils/common_utilities.hpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS_COMMON_UTILITIES_HPP_ -#define PANTHER_UTILS_COMMON_UTILITIES_HPP_ +#ifndef HUSARION_UGV_UTILS_COMMON_UTILITIES_HPP_ +#define HUSARION_UGV_UTILS_COMMON_UTILITIES_HPP_ +#include #include #include #include #include -namespace panther_utils::common_utilities +namespace husarion_ugv_utils::common_utilities { /** @@ -95,7 +96,7 @@ std::map PrefixMapKeys( * * @throws std::runtime_error if the file fails to open. */ -std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmode & mode) +inline std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmode & mode) { std::fstream file(file_path, mode); if (!file.is_open()) { @@ -104,6 +105,22 @@ std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmo return file; } -} // namespace panther_utils::common_utilities +/** + * @brief Checks if required version is satisfied. + * + * @param version Version to be verified. + * @param required_version The required version. + * @return true if the required version is satisfied, false otherwise. + */ +inline bool MeetsVersionRequirement(const float version, const float required_version) +{ + if (std::isnan(version) || std::isnan(required_version)) { + return false; + } + + return version >= required_version - std::numeric_limits::epsilon(); +} + +} // namespace husarion_ugv_utils::common_utilities -#endif // PANTHER_UTILS_COMMON_UTILITIES_HPP_ +#endif // HUSARION_UGV_UTILS_COMMON_UTILITIES_HPP_ diff --git a/panther_utils/include/panther_utils/configure_rt.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/configure_rt.hpp similarity index 87% rename from panther_utils/include/panther_utils/configure_rt.hpp rename to husarion_ugv_utils/include/husarion_ugv_utils/configure_rt.hpp index 1d6d58ae..6420b6bf 100644 --- a/panther_utils/include/panther_utils/configure_rt.hpp +++ b/husarion_ugv_utils/include/husarion_ugv_utils/configure_rt.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS_CONFIGURE_RT_HPP_ -#define PANTHER_UTILS_CONFIGURE_RT_HPP_ +#ifndef HUSARION_UGV_UTILS_CONFIGURE_RT_HPP_ +#define HUSARION_UGV_UTILS_CONFIGURE_RT_HPP_ #include #include "realtime_tools/thread_priority.hpp" -namespace panther_utils +namespace husarion_ugv_utils { /** * @brief Configures thread that calls this function to FIFO scheduler with RT priority @@ -44,6 +44,6 @@ inline void ConfigureRT(const unsigned priority) } } -} // namespace panther_utils +} // namespace husarion_ugv_utils -#endif // PANTHER_UTILS_CONFIGURE_RT_HPP_ +#endif // HUSARION_UGV_UTILS_CONFIGURE_RT_HPP_ diff --git a/panther_utils/include/panther_utils/diagnostics.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/diagnostics.hpp similarity index 88% rename from panther_utils/include/panther_utils/diagnostics.hpp rename to husarion_ugv_utils/include/husarion_ugv_utils/diagnostics.hpp index 937a8fc2..f1278f4c 100644 --- a/panther_utils/include/panther_utils/diagnostics.hpp +++ b/husarion_ugv_utils/include/husarion_ugv_utils/diagnostics.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS_DIAGNOSTICS_HPP_ -#define PANTHER_UTILS_DIAGNOSTICS_HPP_ +#ifndef HUSARION_UGV_UTILS_DIAGNOSTICS_HPP_ +#define HUSARION_UGV_UTILS_DIAGNOSTICS_HPP_ #include #include #include "diagnostic_updater/diagnostic_status_wrapper.hpp" -namespace panther_utils::diagnostics +namespace husarion_ugv_utils::diagnostics { /** @@ -49,6 +49,6 @@ void AddKeyValueIfTrue( } } -} // namespace panther_utils::diagnostics +} // namespace husarion_ugv_utils::diagnostics -#endif // PANTHER_UTILS_DIAGNOSTICS_HPP_ +#endif // HUSARION_UGV_UTILS_DIAGNOSTICS_HPP_ diff --git a/panther_utils/include/panther_utils/moving_average.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp similarity index 55% rename from panther_utils/include/panther_utils/moving_average.hpp rename to husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp index 7575c562..fc1d6b4f 100644 --- a/panther_utils/include/panther_utils/moving_average.hpp +++ b/husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS_MOVING_AVERAGE_HPP_ -#define PANTHER_UTILS_MOVING_AVERAGE_HPP_ +#ifndef HUSARION_UGV_UTILS_MOVING_AVERAGE_HPP_ +#define HUSARION_UGV_UTILS_MOVING_AVERAGE_HPP_ #include +#include -namespace panther_utils +namespace husarion_ugv_utils { template @@ -25,42 +26,42 @@ class MovingAverage { public: MovingAverage(const std::size_t window_size = 5, const T initial_value = T(0)) - : window_size_(window_size), initial_value_(initial_value), sum_(T(0)) + : window_size_(window_size), initial_value_(initial_value) { + if (window_size_ == 0) { + throw std::invalid_argument("Window size must be greater than 0"); + } } void Roll(const T value) { - values_.push_back(value); - sum_ += value; + buffer_.push_back(value); - if (values_.size() > window_size_) { - sum_ -= values_.front(); - values_.pop_front(); + if (buffer_.size() > window_size_) { + buffer_.pop_front(); } } - void Reset() - { - values_.erase(values_.begin(), values_.end()); - sum_ = T(0); - } + void Reset() { buffer_.erase(buffer_.begin(), buffer_.end()); } T GetAverage() const { - if (values_.size() == 0) { + if (buffer_.size() == 0) { return initial_value_; } - return sum_ / static_cast(values_.size()); + + T sum = std::accumulate(buffer_.begin(), buffer_.end(), T(0)); + T average = sum / static_cast(buffer_.size()); + + return average; } private: const std::size_t window_size_; - std::deque values_; + std::deque buffer_; const T initial_value_; - T sum_; }; -} // namespace panther_utils +} // namespace husarion_ugv_utils -#endif // PANTHER_UTILS_MOVING_AVERAGE_HPP_ +#endif // HUSARION_UGV_UTILS_MOVING_AVERAGE_HPP_ diff --git a/panther_utils/include/panther_utils/ros_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp similarity index 94% rename from panther_utils/include/panther_utils/ros_utils.hpp rename to husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp index d8157bc1..6d226ca7 100644 --- a/panther_utils/include/panther_utils/ros_utils.hpp +++ b/husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS__ROS_UTILS_HPP_ -#define PANTHER_UTILS__ROS_UTILS_HPP_ +#ifndef HUSARION_UGV_UTILS__ROS_UTILS_HPP_ +#define HUSARION_UGV_UTILS__ROS_UTILS_HPP_ #include #include "std_msgs/msg/header.hpp" -namespace panther_utils::ros +namespace husarion_ugv_utils::ros { /** @@ -105,6 +105,6 @@ std::string AddNamespaceToFrameID(const std::string & frame_id, const std::strin return tf_prefix + frame_id; } -} // namespace panther_utils::ros +} // namespace husarion_ugv_utils::ros -#endif // PANTHER_UTILS__ROS_UTILS_HPP_ +#endif // HUSARION_UGV_UTILS__ROS_UTILS_HPP_ diff --git a/panther_utils/include/panther_utils/test/ros_test_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/test/ros_test_utils.hpp similarity index 93% rename from panther_utils/include/panther_utils/test/ros_test_utils.hpp rename to husarion_ugv_utils/include/husarion_ugv_utils/test/ros_test_utils.hpp index c6a6b728..aaef6de1 100644 --- a/panther_utils/include/panther_utils/test/ros_test_utils.hpp +++ b/husarion_ugv_utils/include/husarion_ugv_utils/test/ros_test_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS_TEST_UTILS_HPP_ -#define PANTHER_UTILS_TEST_UTILS_HPP_ +#ifndef HUSARION_UGV_UTILS_TEST_UTILS_HPP_ +#define HUSARION_UGV_UTILS_TEST_UTILS_HPP_ #include #include @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" -namespace panther_utils::test_utils +namespace husarion_ugv_utils::test_utils { /** @@ -100,6 +100,6 @@ void PublishAndSpin( std::this_thread::sleep_for(std::chrono::milliseconds(10)); } -} // namespace panther_utils::test_utils +} // namespace husarion_ugv_utils::test_utils -#endif // PANTHER_UTILS_TEST_UTILS_HPP_ +#endif // HUSARION_UGV_UTILS_TEST_UTILS_HPP_ diff --git a/panther_utils/include/panther_utils/test/test_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/test/test_utils.hpp similarity index 90% rename from panther_utils/include/panther_utils/test/test_utils.hpp rename to husarion_ugv_utils/include/husarion_ugv_utils/test/test_utils.hpp index a71322c0..1346f5f4 100644 --- a/panther_utils/include/panther_utils/test/test_utils.hpp +++ b/husarion_ugv_utils/include/husarion_ugv_utils/test/test_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS_TEST_UTILS_HPP_ -#define PANTHER_UTILS_TEST_UTILS_HPP_ +#ifndef HUSARION_UGV_UTILS_TEST_UTILS_HPP_ +#define HUSARION_UGV_UTILS_TEST_UTILS_HPP_ #include #include @@ -22,7 +22,7 @@ #include #include -namespace panther_utils::test_utils +namespace husarion_ugv_utils::test_utils { /** @@ -69,6 +69,6 @@ bool IsMessageThrown(const Func & func, const std::string & error_msg) return false; } -} // namespace panther_utils::test_utils +} // namespace husarion_ugv_utils::test_utils -#endif // PANTHER_UTILS_TEST_UTILS_HPP_ +#endif // HUSARION_UGV_UTILS_TEST_UTILS_HPP_ diff --git a/panther_utils/include/panther_utils/yaml_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/yaml_utils.hpp similarity index 91% rename from panther_utils/include/panther_utils/yaml_utils.hpp rename to husarion_ugv_utils/include/husarion_ugv_utils/yaml_utils.hpp index e6137bda..8d32559a 100644 --- a/panther_utils/include/panther_utils/yaml_utils.hpp +++ b/husarion_ugv_utils/include/husarion_ugv_utils/yaml_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS_YAML_UTILS_HPP_ -#define PANTHER_UTILS_YAML_UTILS_HPP_ +#ifndef HUSARION_UGV_UTILS_YAML_UTILS_HPP_ +#define HUSARION_UGV_UTILS_YAML_UTILS_HPP_ #include #include #include "yaml-cpp/yaml.h" -namespace panther_utils +namespace husarion_ugv_utils { /** @@ -69,6 +69,6 @@ T GetYAMLKeyValue(const YAML::Node & description, const std::string & key, const } } -} // namespace panther_utils +} // namespace husarion_ugv_utils -#endif // PANTHER_UTILS_YAML_UTILS_HPP_ +#endif // HUSARION_UGV_UTILS_YAML_UTILS_HPP_ diff --git a/panther_utils/package.xml b/husarion_ugv_utils/package.xml similarity index 97% rename from panther_utils/package.xml rename to husarion_ugv_utils/package.xml index 53fd30f8..f58e72c6 100644 --- a/panther_utils/package.xml +++ b/husarion_ugv_utils/package.xml @@ -1,7 +1,7 @@ - panther_utils + husarion_ugv_utils 2.1.2 Package for commonly used functions, classes, and configurations Husarion diff --git a/panther_utils/test/test_common_utilities.cpp b/husarion_ugv_utils/test/test_common_utilities.cpp similarity index 59% rename from panther_utils/test/test_common_utilities.cpp rename to husarion_ugv_utils/test/test_common_utilities.cpp index 0527b69b..fe374936 100644 --- a/panther_utils/test/test_common_utilities.cpp +++ b/husarion_ugv_utils/test/test_common_utilities.cpp @@ -20,14 +20,14 @@ #include "gtest/gtest.h" -#include "panther_utils/common_utilities.hpp" +#include "husarion_ugv_utils/common_utilities.hpp" TEST(TestSetPrecision, TwoDigitPrecision) { float value = 3.14159; float expected_result = 3.14; - float result = panther_utils::common_utilities::SetPrecision(value, 2); + float result = husarion_ugv_utils::common_utilities::SetPrecision(value, 2); EXPECT_FLOAT_EQ(expected_result, result); } @@ -37,7 +37,7 @@ TEST(TestSetPrecision, ZeroDigitPrecision) float value = 3.54159; float expected_result = 4.0; - float result = panther_utils::common_utilities::SetPrecision(value, 0); + float result = husarion_ugv_utils::common_utilities::SetPrecision(value, 0); EXPECT_FLOAT_EQ(expected_result, result); } @@ -46,7 +46,7 @@ TEST(TestSetPrecision, NegativeValue) { float value = -3.14159; float expected_result = -3.14; - float result = panther_utils::common_utilities::SetPrecision(value, 2); + float result = husarion_ugv_utils::common_utilities::SetPrecision(value, 2); EXPECT_FLOAT_EQ(expected_result, result); } @@ -54,7 +54,7 @@ TEST(TestSetPrecision, LargeValue) { float value = 123456.789; float expected_result = 123456.79; - float result = panther_utils::common_utilities::SetPrecision(value, 2); + float result = husarion_ugv_utils::common_utilities::SetPrecision(value, 2); EXPECT_FLOAT_EQ(expected_result, result); } @@ -64,7 +64,7 @@ TEST(TestCountPercentage, ValidValues) int total = 100; float expected_result = 25.00; - float result = panther_utils::common_utilities::CountPercentage(value, total); + float result = husarion_ugv_utils::common_utilities::CountPercentage(value, total); EXPECT_FLOAT_EQ(expected_result, result); } @@ -75,7 +75,7 @@ TEST(TestCountPercentage, ZeroTotal) int total = 0; EXPECT_THROW( - panther_utils::common_utilities::CountPercentage(value, total), std::invalid_argument); + husarion_ugv_utils::common_utilities::CountPercentage(value, total), std::invalid_argument); } TEST(TestCountPercentage, ZeroValue) @@ -84,7 +84,7 @@ TEST(TestCountPercentage, ZeroValue) int total = 100; float expected_result = 0.00; - float result = panther_utils::common_utilities::CountPercentage(value, total); + float result = husarion_ugv_utils::common_utilities::CountPercentage(value, total); EXPECT_FLOAT_EQ(expected_result, result); } @@ -95,7 +95,7 @@ TEST(TestPrefixMapKeys, CorrectlyPrefixesKeys) std::string prefix = "prefix_"; std::map expected_map = {{"prefix_key1", 1}, {"prefix_key2", 2}}; - auto result_map = panther_utils::common_utilities::PrefixMapKeys(input_map, prefix); + auto result_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(input_map, prefix); EXPECT_EQ(result_map, expected_map); } @@ -106,7 +106,7 @@ TEST(TestPrefixMapKeys, HandlesEmptyPrefix) std::string prefix = ""; std::map expected_map = input_map; // The maps should be identical - auto result_map = panther_utils::common_utilities::PrefixMapKeys(input_map, prefix); + auto result_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(input_map, prefix); EXPECT_EQ(result_map, expected_map); } @@ -117,7 +117,7 @@ TEST(TestPrefixMapKeys, HandlesEmptyMap) std::string prefix = "prefix_"; std::map expected_map; - auto result_map = panther_utils::common_utilities::PrefixMapKeys(input_map, prefix); + auto result_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(input_map, prefix); EXPECT_EQ(result_map, expected_map); } @@ -128,14 +128,14 @@ TEST(TestPrefixMapKeys, HandlesNonAlphanumericPrefix) std::string prefix = "prefix_@#"; std::map expected_map = {{"prefix_@#key1", 1}, {"prefix_@#key2", 2}}; - auto result_map = panther_utils::common_utilities::PrefixMapKeys(input_map, prefix); + auto result_map = husarion_ugv_utils::common_utilities::PrefixMapKeys(input_map, prefix); EXPECT_EQ(result_map, expected_map); } TEST(TestOpenFile, HandleOpenFile) { - std::string path = testing::TempDir() + "test_panther_utils_open_file"; + std::string path = testing::TempDir() + "test_husarion_ugv_utils_open_file"; // Make sure that there is no random file. std::filesystem::remove(path); @@ -143,19 +143,56 @@ TEST(TestOpenFile, HandleOpenFile) std::ofstream ofs(path); ofs.close(); - EXPECT_NO_THROW({ panther_utils::common_utilities::OpenFile(path, std::ios_base::out); }); + EXPECT_NO_THROW({ husarion_ugv_utils::common_utilities::OpenFile(path, std::ios_base::out); }); std::filesystem::remove(path); } TEST(TestOpenFile, HandleOpenFileThrow) { - std::string path = testing::TempDir() + "test_panther_utils_open_file"; + std::string path = testing::TempDir() + "test_husarion_ugv_utils_open_file"; // Make sure that there is no random file. std::filesystem::remove(path); EXPECT_THROW( - { panther_utils::common_utilities::OpenFile(path, std::ios_base::in); }, std::runtime_error); + { husarion_ugv_utils::common_utilities::OpenFile(path, std::ios_base::in); }, + std::runtime_error); +} + +TEST(TestMeetsVersionRequirement, SameVersion) +{ + float version = 1.06; + + auto is_met = husarion_ugv_utils::common_utilities::MeetsVersionRequirement(version, 1.06); + + EXPECT_TRUE(is_met); +} + +TEST(TestMeetsVersionRequirement, LowerVersion) +{ + float version = 1.2; + + auto is_met = husarion_ugv_utils::common_utilities::MeetsVersionRequirement(version, 1.06); + + EXPECT_TRUE(is_met); +} + +TEST(TestMeetsVersionRequirement, HigherVersion) +{ + float version = 1.0; + + auto is_met = husarion_ugv_utils::common_utilities::MeetsVersionRequirement(version, 1.06); + + EXPECT_FALSE(is_met); +} + +TEST(TestMeetsVersionRequirement, NaNVersionRequired) +{ + float version = std::numeric_limits::quiet_NaN(); + + auto is_met = husarion_ugv_utils::common_utilities::MeetsVersionRequirement(version, 1.06); + + EXPECT_FALSE(is_met); } int main(int argc, char ** argv) diff --git a/panther_utils/test/test_diagnostics.cpp b/husarion_ugv_utils/test/test_diagnostics.cpp similarity index 84% rename from panther_utils/test/test_diagnostics.cpp rename to husarion_ugv_utils/test/test_diagnostics.cpp index c09eddc4..993275d8 100644 --- a/panther_utils/test/test_diagnostics.cpp +++ b/husarion_ugv_utils/test/test_diagnostics.cpp @@ -20,14 +20,14 @@ #include "diagnostic_updater/diagnostic_status_wrapper.hpp" -#include "panther_utils/diagnostics.hpp" +#include "husarion_ugv_utils/diagnostics.hpp" TEST(TestAddKeyValueIfTrue, HandlesBoolean) { diagnostic_updater::DiagnosticStatusWrapper status; std::map kv_map = {{"key1", true}, {"key2", false}, {"key3", true}}; - panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map); + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map); ASSERT_EQ(2, status.values.size()); EXPECT_EQ("key1", status.values.at(0).key); @@ -40,7 +40,7 @@ TEST(TestAddKeyValueIfTrue, HandlesSmartPointer) std::map> kv_map = { {"key1", nullptr}, {"key2", nullptr}, {"key3", std::make_shared(1)}}; - panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map); + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map); ASSERT_EQ(1, status.values.size()); EXPECT_EQ("key3", status.values.at(0).key); @@ -51,7 +51,7 @@ TEST(TestAddKeyValueIfTrue, HandlesInteger) diagnostic_updater::DiagnosticStatusWrapper status; std::map kv_map = {{"key1", 0}, {"key2", 1}, {"key3", 2}}; - panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map); + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map); ASSERT_EQ(2, status.values.size()); EXPECT_EQ("key2", status.values.at(0).key); @@ -63,7 +63,7 @@ TEST(TestAddKeyValueIfTrue, HandlesEmptyMap) diagnostic_updater::DiagnosticStatusWrapper status; std::map kv_map; - panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map); + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map); ASSERT_EQ(0, status.values.size()); } @@ -73,7 +73,7 @@ TEST(TestAddKeyValueIfTrue, HandlesPrefixAndSuffix) diagnostic_updater::DiagnosticStatusWrapper status; std::map kv_map = {{"key1", true}, {"key2", false}, {"key3", true}}; - panther_utils::diagnostics::AddKeyValueIfTrue(status, kv_map, "prefix_", "_suffix"); + husarion_ugv_utils::diagnostics::AddKeyValueIfTrue(status, kv_map, "prefix_", "_suffix"); ASSERT_EQ(2, status.values.size()); EXPECT_EQ("prefix_key1_suffix", status.values.at(0).key); diff --git a/panther_utils/test/test_moving_average.cpp b/husarion_ugv_utils/test/test_moving_average.cpp similarity index 66% rename from panther_utils/test/test_moving_average.cpp rename to husarion_ugv_utils/test/test_moving_average.cpp index f102a7b2..2934f28e 100644 --- a/panther_utils/test/test_moving_average.cpp +++ b/husarion_ugv_utils/test/test_moving_average.cpp @@ -14,29 +14,29 @@ #include "gtest/gtest.h" -#include "panther_utils/moving_average.hpp" +#include "husarion_ugv_utils/moving_average.hpp" TEST(TestMovingAverage, TestDefaultInitialValue) { - panther_utils::MovingAverage ma(4); + husarion_ugv_utils::MovingAverage ma(4); EXPECT_EQ(0.0, ma.GetAverage()); } TEST(TestMovingAverage, TestIntialValue) { - panther_utils::MovingAverage ma(4, 1.0); + husarion_ugv_utils::MovingAverage ma(4, 1.0); EXPECT_EQ(1.0, ma.GetAverage()); - panther_utils::MovingAverage ma_1(10, 3.7); + husarion_ugv_utils::MovingAverage ma_1(10, 3.7); EXPECT_EQ(3.7, ma_1.GetAverage()); - panther_utils::MovingAverage ma_2(3, 4); + husarion_ugv_utils::MovingAverage ma_2(3, 4); EXPECT_EQ(4, ma_2.GetAverage()); } TEST(TestMovingAverage, TestOutputValues) { - panther_utils::MovingAverage ma(4); + husarion_ugv_utils::MovingAverage ma(4); ma.Roll(1.0); ASSERT_EQ(1.0, ma.GetAverage()); @@ -57,7 +57,7 @@ TEST(TestMovingAverage, TestOutputValues) TEST(TestMovingAverage, TestHighOverload) { const std::size_t window_len = 1000; - panther_utils::MovingAverage ma(window_len); + husarion_ugv_utils::MovingAverage ma(window_len); double sum = 0.0; for (std::size_t i = 1; i <= window_len * 10; i++) { @@ -66,7 +66,8 @@ TEST(TestMovingAverage, TestHighOverload) // test every 1000 rolls expected average if (i % window_len == 0) { - EXPECT_EQ(sum / double(window_len), ma.GetAverage()); + ASSERT_NEAR( + sum / double(window_len), ma.GetAverage(), std::numeric_limits::epsilon()); sum = 0.0; } } @@ -74,7 +75,7 @@ TEST(TestMovingAverage, TestHighOverload) TEST(TestMovingAverage, TestIntFloorRound) { - panther_utils::MovingAverage ma; + husarion_ugv_utils::MovingAverage ma; ma.Roll(1); ma.Roll(2); EXPECT_EQ(1, ma.GetAverage()); @@ -82,7 +83,7 @@ TEST(TestMovingAverage, TestIntFloorRound) TEST(TestMovingAverage, TestReset) { - panther_utils::MovingAverage ma(4); + husarion_ugv_utils::MovingAverage ma(4); ma.Roll(1.0); ma.Roll(2.0); ma.Roll(1.0); @@ -99,7 +100,7 @@ TEST(TestMovingAverage, TestReset) TEST(TestMovingAverage, TestResetToInitialValue) { - panther_utils::MovingAverage ma(4, 7.0); + husarion_ugv_utils::MovingAverage ma(4, 7.0); ma.Roll(1.0); ma.Roll(2.0); EXPECT_EQ(1.5, ma.GetAverage()); @@ -107,6 +108,25 @@ TEST(TestMovingAverage, TestResetToInitialValue) EXPECT_EQ(7.0, ma.GetAverage()); } +TEST(TestMovingAverage, TestInfInjectionHandling) +{ + husarion_ugv_utils::MovingAverage ma(4); + ma.Roll(1.0); + ma.Roll(2.0); + ma.Roll(3.0); + ma.Roll(4.0); + EXPECT_EQ(2.5, ma.GetAverage()); + + ma.Roll(std::numeric_limits::infinity()); + EXPECT_EQ(std::numeric_limits::infinity(), ma.GetAverage()); + + ma.Roll(1.0); + ma.Roll(2.0); + ma.Roll(3.0); + ma.Roll(4.0); + EXPECT_EQ(2.5, ma.GetAverage()); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/panther_utils/test/test_ros_test_utils.cpp b/husarion_ugv_utils/test/test_ros_test_utils.cpp similarity index 84% rename from panther_utils/test/test_ros_test_utils.cpp rename to husarion_ugv_utils/test/test_ros_test_utils.cpp index d47d2d13..04ea84ac 100644 --- a/panther_utils/test/test_ros_test_utils.cpp +++ b/husarion_ugv_utils/test/test_ros_test_utils.cpp @@ -21,7 +21,7 @@ #include "std_msgs/msg/empty.hpp" -#include "panther_utils/test/ros_test_utils.hpp" +#include "husarion_ugv_utils/test/ros_test_utils.hpp" TEST(TestROSTestUtils, WaitForMessage) { @@ -34,11 +34,11 @@ TEST(TestROSTestUtils, WaitForMessage) topic_name, 10, [&](const std_msgs::msg::Empty::SharedPtr msg) { empty_msg = msg; }); EXPECT_FALSE( - panther_utils::test_utils::WaitForMsg(node, empty_msg, std::chrono::milliseconds(1000))); + husarion_ugv_utils::test_utils::WaitForMsg(node, empty_msg, std::chrono::milliseconds(1000))); pub->publish(std_msgs::msg::Empty()); EXPECT_TRUE( - panther_utils::test_utils::WaitForMsg(node, empty_msg, std::chrono::milliseconds(1000))); + husarion_ugv_utils::test_utils::WaitForMsg(node, empty_msg, std::chrono::milliseconds(1000))); } TEST(TestROSTestUtils, PublishAndSpin) @@ -53,7 +53,7 @@ TEST(TestROSTestUtils, PublishAndSpin) EXPECT_FALSE(received_msg); - panther_utils::test_utils::PublishAndSpin(node, topic_name, published_msg); + husarion_ugv_utils::test_utils::PublishAndSpin(node, topic_name, published_msg); EXPECT_TRUE(received_msg); } diff --git a/panther_utils/test/test_ros_utils.cpp b/husarion_ugv_utils/test/test_ros_utils.cpp similarity index 76% rename from panther_utils/test/test_ros_utils.cpp rename to husarion_ugv_utils/test/test_ros_utils.cpp index 5832388a..eb3e470d 100644 --- a/panther_utils/test/test_ros_utils.cpp +++ b/husarion_ugv_utils/test/test_ros_utils.cpp @@ -17,7 +17,7 @@ #include -#include "panther_utils/ros_utils.hpp" +#include "husarion_ugv_utils/ros_utils.hpp" using HeaderMsg = std_msgs::msg::Header; @@ -32,7 +32,8 @@ TEST(TestVerifyTimestampGap, TimestampGapOk) header_2.stamp.nanosec = 500000000; auto max_timestamp_gap = std::chrono::seconds(2); - EXPECT_NO_THROW(panther_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap)); + EXPECT_NO_THROW( + husarion_ugv_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap)); } TEST(TestVerifyTimestampGap, TimestampGapExceeding) @@ -48,7 +49,7 @@ TEST(TestVerifyTimestampGap, TimestampGapExceeding) auto max_timestamp_gap = std::chrono::seconds(1); EXPECT_THROW( - panther_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap), + husarion_ugv_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap), std::runtime_error); } @@ -60,7 +61,7 @@ TEST(TestVerifyTimestampGap, TimestampNotSet) auto max_timestamp_gap = std::chrono::seconds(1); EXPECT_THROW( - panther_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap), + husarion_ugv_utils::ros::VerifyTimestampGap(header_1, header_2, max_timestamp_gap), std::runtime_error); } @@ -74,7 +75,7 @@ TEST(TestMergeHeaders, SameFrameIds) HeaderMsg merged_header; - EXPECT_NO_THROW(merged_header = panther_utils::ros::MergeHeaders(header_1, header_2)); + EXPECT_NO_THROW(merged_header = husarion_ugv_utils::ros::MergeHeaders(header_1, header_2)); EXPECT_STREQ("frame", merged_header.frame_id.c_str()); } @@ -86,7 +87,7 @@ TEST(TestMergeHeaders, DifferentFrameIds) HeaderMsg header_2; header_2.frame_id = "frame_2"; - EXPECT_THROW(panther_utils::ros::MergeHeaders(header_1, header_2), std::runtime_error); + EXPECT_THROW(husarion_ugv_utils::ros::MergeHeaders(header_1, header_2), std::runtime_error); } TEST(TestMergeHeaders, EarlierTimestampNanosec) @@ -99,7 +100,7 @@ TEST(TestMergeHeaders, EarlierTimestampNanosec) header_2.stamp.sec = 10; header_2.stamp.nanosec = 500000000; - auto merged_header = panther_utils::ros::MergeHeaders(header_1, header_2); + auto merged_header = husarion_ugv_utils::ros::MergeHeaders(header_1, header_2); EXPECT_EQ(merged_header.stamp.sec, 10); EXPECT_EQ(merged_header.stamp.nanosec, 200000000); @@ -115,7 +116,7 @@ TEST(TestMergeHeaders, EarlierTimestampSec) header_2.stamp.sec = 10; header_2.stamp.nanosec = 500000000; - auto merged_header = panther_utils::ros::MergeHeaders(header_1, header_2); + auto merged_header = husarion_ugv_utils::ros::MergeHeaders(header_1, header_2); EXPECT_EQ(merged_header.stamp.sec, 9); EXPECT_EQ(merged_header.stamp.nanosec, 500000000); @@ -131,7 +132,7 @@ TEST(TestMergeHeaders, LaterTimestamp) header_2.stamp.sec = 9; header_2.stamp.nanosec = 500000000; - auto merged_header = panther_utils::ros::MergeHeaders(header_1, header_2); + auto merged_header = husarion_ugv_utils::ros::MergeHeaders(header_1, header_2); EXPECT_EQ(merged_header.stamp.sec, 9); EXPECT_EQ(merged_header.stamp.nanosec, 500000000); @@ -142,7 +143,7 @@ TEST(TestAddNamespaceToFrameID, WithoutNamespace) std::string frame_id = "frame"; std::string ns = ""; - auto result = panther_utils::ros::AddNamespaceToFrameID(frame_id, ns); + auto result = husarion_ugv_utils::ros::AddNamespaceToFrameID(frame_id, ns); EXPECT_EQ(result, "frame"); } @@ -151,7 +152,7 @@ TEST(TestAddNamespaceToFrameID, WithSlashNamespace) std::string frame_id = "frame"; std::string ns = "/"; - auto result = panther_utils::ros::AddNamespaceToFrameID(frame_id, ns); + auto result = husarion_ugv_utils::ros::AddNamespaceToFrameID(frame_id, ns); EXPECT_EQ(result, "frame"); } @@ -160,7 +161,7 @@ TEST(TestAddNamespaceToFrameID, WithSlashedNamespace) std::string frame_id = "frame"; std::string ns = "/namespace"; - auto result = panther_utils::ros::AddNamespaceToFrameID(frame_id, ns); + auto result = husarion_ugv_utils::ros::AddNamespaceToFrameID(frame_id, ns); EXPECT_EQ(result, "namespace/frame"); } @@ -169,7 +170,7 @@ TEST(TestAddNamespaceToFrameID, WithNamespace) std::string frame_id = "frame"; std::string ns = "namespace"; - auto result = panther_utils::ros::AddNamespaceToFrameID(frame_id, ns); + auto result = husarion_ugv_utils::ros::AddNamespaceToFrameID(frame_id, ns); EXPECT_EQ(result, "namespace/frame"); } diff --git a/panther_utils/test/test_test_utils.cpp b/husarion_ugv_utils/test/test_test_utils.cpp similarity index 66% rename from panther_utils/test/test_test_utils.cpp rename to husarion_ugv_utils/test/test_test_utils.cpp index 4f319edc..fd9b0a36 100644 --- a/panther_utils/test/test_test_utils.cpp +++ b/husarion_ugv_utils/test/test_test_utils.cpp @@ -18,15 +18,15 @@ #include "gtest/gtest.h" -#include "panther_utils/test/test_utils.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" template void TestCheckNaNVector() { std::vector vector(10, std::numeric_limits::quiet_NaN()); - EXPECT_TRUE(panther_utils::test_utils::CheckNaNVector(vector)); + EXPECT_TRUE(husarion_ugv_utils::test_utils::CheckNaNVector(vector)); vector.push_back(1.0); - EXPECT_FALSE(panther_utils::test_utils::CheckNaNVector(vector)); + EXPECT_FALSE(husarion_ugv_utils::test_utils::CheckNaNVector(vector)); } TEST(TestTestUtils, CheckNanVector) @@ -39,46 +39,46 @@ TEST(TestTestUtils, CheckNanVector) TEST(TestTestUtils, IsMessageThrownTrue) { - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::runtime_error("Example exception"); }, "Example exception")); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::out_of_range("Example exception"); }, "Example exception")); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::invalid_argument("Example exception"); }, "Example exception")); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::runtime_error("Example exception"); }, "Example")); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::runtime_error("Example exception"); }, "exception")); } TEST(TestTestUtils, IsMessageThrownDifferentException) { - EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::out_of_range("Example exception"); }, "Example exception")); - EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::invalid_argument("Example exception"); }, "Example exception")); - EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::runtime_error("Example exception"); }, "Example exception")); } TEST(TestTestUtils, IsMessageThrownDifferentMessage) { - EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::runtime_error("Example exception"); }, "Different exception message")); - EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { throw std::runtime_error("Example exception"); }, "Example exception ")); } TEST(TestTestUtils, IsMessageThrownNoThrow) { - EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + EXPECT_FALSE(husarion_ugv_utils::test_utils::IsMessageThrown( []() { return; }, "Example exception")); } diff --git a/panther_utils/test/test_yaml_utils.cpp b/husarion_ugv_utils/test/test_yaml_utils.cpp similarity index 53% rename from panther_utils/test/test_yaml_utils.cpp rename to husarion_ugv_utils/test/test_yaml_utils.cpp index 8f853350..2f4c62fb 100644 --- a/panther_utils/test/test_yaml_utils.cpp +++ b/husarion_ugv_utils/test/test_yaml_utils.cpp @@ -15,14 +15,14 @@ #include "gtest/gtest.h" #include "yaml-cpp/yaml.h" -#include "panther_utils/test/test_utils.hpp" -#include "panther_utils/yaml_utils.hpp" +#include "husarion_ugv_utils/test/test_utils.hpp" +#include "husarion_ugv_utils/yaml_utils.hpp" TEST(TestGetYAMLKeyValue, MissingKey) { YAML::Node desc; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [desc]() { panther_utils::GetYAMLKeyValue(desc, "key_name"); }, + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [desc]() { husarion_ugv_utils::GetYAMLKeyValue(desc, "key_name"); }, "Missing 'key_name' in description")); } @@ -34,17 +34,19 @@ TEST(TestGetYAMLKeyValue, ConversionFailure) desc["string_key"] = "string"; desc["int_vector_key"] = "[1 2 3]"; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { panther_utils::GetYAMLKeyValue(desc, "float_key"); }, "Failed to convert")); + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { husarion_ugv_utils::GetYAMLKeyValue(desc, "float_key"); }, "Failed to convert")); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { panther_utils::GetYAMLKeyValue(desc, "string_key"); }, "Failed to convert")); + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { husarion_ugv_utils::GetYAMLKeyValue(desc, "string_key"); }, + "Failed to convert")); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { panther_utils::GetYAMLKeyValue(desc, "int_vector_key"); }, "Failed to convert")); + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { husarion_ugv_utils::GetYAMLKeyValue(desc, "int_vector_key"); }, + "Failed to convert")); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { panther_utils::GetYAMLKeyValue>(desc, "string_key"); }, + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { husarion_ugv_utils::GetYAMLKeyValue>(desc, "string_key"); }, "Failed to convert")); } @@ -56,13 +58,13 @@ TEST(TestGetYAMLKeyValue, GetKey) desc["float_key"] = 1.5; desc["string_key"] = "string"; - const auto int_value = panther_utils::GetYAMLKeyValue(desc, "int_key"); + const auto int_value = husarion_ugv_utils::GetYAMLKeyValue(desc, "int_key"); EXPECT_EQ(2, int_value); - const auto float_value = panther_utils::GetYAMLKeyValue(desc, "float_key"); + const auto float_value = husarion_ugv_utils::GetYAMLKeyValue(desc, "float_key"); EXPECT_EQ(1.5, float_value); - const auto str_value = panther_utils::GetYAMLKeyValue(desc, "string_key"); + const auto str_value = husarion_ugv_utils::GetYAMLKeyValue(desc, "string_key"); EXPECT_EQ("string", str_value); } @@ -72,7 +74,7 @@ TEST(TestGetYAMLKeyValue, GetVectorKey) desc["int_vector_key"] = std::vector(5, 147); - const auto value_vector = panther_utils::GetYAMLKeyValue>( + const auto value_vector = husarion_ugv_utils::GetYAMLKeyValue>( desc, "int_vector_key"); for (auto value : value_vector) { EXPECT_EQ(147, value); @@ -84,7 +86,7 @@ TEST(TestGetYAMLKeyValue, GetKeyDefaultValue) YAML::Node desc; const int default_value = 54; - const auto value = panther_utils::GetYAMLKeyValue(desc, "key_name", default_value); + const auto value = husarion_ugv_utils::GetYAMLKeyValue(desc, "key_name", default_value); EXPECT_EQ(default_value, value); } diff --git a/lynx_description/CMakeLists.txt b/lynx_description/CMakeLists.txt new file mode 100644 index 00000000..f4e2ded4 --- /dev/null +++ b/lynx_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.10.2) +project(lynx_description) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY config launch urdf DESTINATION share/${PROJECT_NAME}) + +ament_environment_hooks( + "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in") + +ament_package() diff --git a/lynx_description/README.md b/lynx_description/README.md new file mode 100644 index 00000000..953bab34 --- /dev/null +++ b/lynx_description/README.md @@ -0,0 +1,12 @@ +# lynx_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. +- [`WH05.yaml`](./config/WH05.yaml): Description of physical and visual parameters for the wheel WH05. diff --git a/lynx_description/config/WH05.yaml b/lynx_description/config/WH05.yaml new file mode 100644 index 00000000..4e25988b --- /dev/null +++ b/lynx_description/config/WH05.yaml @@ -0,0 +1,10 @@ +wheel_radius: 0.1348 +wheel_width: 0.09 +wheel_separation: 0.45 +# TODO change inertia and mass when ready +mass: 2.5 +inertia: { ixx: 0.014738, iyy: 0.0261, izz: 0.014738 } +inertia_y_offset: 0.0 +mesh_package: lynx_description +folder_path: meshes/WH05 +mecanum: False diff --git a/lynx_description/config/components.yaml b/lynx_description/config/components.yaml new file mode 100644 index 00000000..71826593 --- /dev/null +++ b/lynx_description/config/components.yaml @@ -0,0 +1,25 @@ +# By default lynx is loaded without any components. + +components: [] + +# If you want to add for example LDR01, LDR06 and CAM01 to your Panther look at this example below: + +# components: +# - type: LDR01 +# parent_link: cover_link +# xyz: 0.0 0.1 0.0 +# rpy: 0.0 0.0 0.0 +# device_namespace: main_lidar + +# - type: LDR06 +# parent_link: cover_link +# xyz: 0.0 -0.1 0.0 +# rpy: 0.0 0.0 0.0 +# device_namespace: second_lidar + +# - type: CAM01 +# name: camera +# parent_link: cover_link +# xyz: 0.1 0.0 0.0 +# rpy: 0.0 0.0 0.0 +# device_namespace: front_cam diff --git a/lynx_description/hooks/lynx_description.sh.in b/lynx_description/hooks/lynx_description.sh.in new file mode 100755 index 00000000..45d23a05 --- /dev/null +++ b/lynx_description/hooks/lynx_description.sh.in @@ -0,0 +1,3 @@ +ament_prepend_unique_value GAZEBO_MODEL_PATH "@CMAKE_INSTALL_PREFIX@/share" +ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share" +ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share" diff --git a/lynx_description/launch/load_urdf.launch.py b/lynx_description/launch/load_urdf.launch.py new file mode 100644 index 00000000..cc3e6dbf --- /dev/null +++ b/lynx_description/launch/load_urdf.launch.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 + +# Copyright 2020 ros2_control Development Team +# 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. + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import ( + Command, + EnvironmentVariable, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + add_wheel_joints = LaunchConfiguration("add_wheel_joints") + declared_add_wheel_joints_arg = DeclareLaunchArgument( + "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", "true", "False", "false"], + ) + + battery_config_path = LaunchConfiguration("battery_config_path") + declare_battery_config_path_arg = DeclareLaunchArgument( + "battery_config_path", + description=( + "Path to the Ignition LinearBatteryPlugin configuration file. " + "This configuration is intended for use in simulations only." + ), + default_value="", + ) + + components_config_path = LaunchConfiguration("components_config_path") + declare_components_config_path_arg = DeclareLaunchArgument( + "components_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("lynx_description"), "config", "components.yaml"] + ), + description=( + "Additional components configuration file. Components described in this file " + "are dynamically included in Lynx's urdf." + "Lynx options are described here " + "https://husarion.com/manuals/lynx/lynx-options/" + ), + ) + + wheel_type = LaunchConfiguration("wheel_type") + controller_config_path = LaunchConfiguration("controller_config_path") + declare_controller_config_path_arg = DeclareLaunchArgument( + "controller_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("husarion_ugv_controller"), + "config", + PythonExpression(["'", wheel_type, "_controller.yaml'"]), + ] + ), + description=( + "Path to controller configuration file. By default, it is located in" + " 'husarion_ugv_controller/config/{wheel_type}_controller.yaml'. You can also specify" + " the path to your custom controller configuration file here. " + ), + ) + + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Add namespace to all launched nodes.", + ) + + use_sim = LaunchConfiguration("use_sim") + declare_use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="False", + description="Whether simulation is used.", + choices=["True", "true", "False", "false"], + ) + + wheel_config_path = LaunchConfiguration("wheel_config_path") + declare_wheel_config_path_arg = DeclareLaunchArgument( + "wheel_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("lynx_description"), + "config", + PythonExpression(["'", wheel_type, ".yaml'"]), + ] + ), + description=( + "Path to wheel configuration file. By default, it is located in " + "'lynx_description/config/{wheel_type}.yaml'. You can also specify the path " + "to your custom wheel configuration file here. " + ), + ) + + declare_wheel_type_arg = DeclareLaunchArgument( + "wheel_type", + default_value="WH05", + description=( + "Specify the wheel type. If the selected wheel type is not 'custom', " + "the 'wheel_config_path' and 'controller_config_path' arguments will be " + "automatically adjusted and can be omitted." + ), + choices=["WH05", "custom"], + ) + + # Get URDF via xacro + imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168") + imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028") + imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083") + imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14") + imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57") + imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0") + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("lynx_description"), "urdf", "lynx.urdf.xacro"] + ), + " use_sim:=", + use_sim, + " wheel_config_file:=", + wheel_config_path, + " controller_config_file:=", + controller_config_path, + " battery_config_file:=", + battery_config_path, + " imu_xyz:=", + f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'", + " imu_rpy:=", + f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'", + " namespace:=", + namespace, + " components_config_path:=", + components_config_path, + ] + ) + + namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"]) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[ + {"robot_description": robot_description_content}, + {"frame_prefix": namespace_ext}, + ], + namespace=namespace, + emulate_tty=True, + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher", + executable="joint_state_publisher", + namespace=namespace, + emulate_tty=True, + condition=IfCondition(add_wheel_joints), + ) + + actions = [ + declared_add_wheel_joints_arg, + declare_battery_config_path_arg, + declare_components_config_path_arg, + declare_wheel_type_arg, # wheel_type is used by controller_config_path + declare_controller_config_path_arg, + declare_namespace_arg, + declare_use_sim_arg, + declare_wheel_config_path_arg, + SetParameter(name="use_sim_time", value=use_sim), + robot_state_pub_node, + joint_state_publisher_node, # do not publish, when use_sim is true + ] + + return LaunchDescription(actions) diff --git a/lynx_description/package.xml b/lynx_description/package.xml new file mode 100644 index 00000000..77584066 --- /dev/null +++ b/lynx_description/package.xml @@ -0,0 +1,28 @@ + + + + lynx_description + 2.1.0 + The lynx_description package + Husarion + Apache License 2.0 + + https://husarion.com/ + https://github.com/husarion/panther_ros + https://github.com/husarion/panther_ros/issues + + Dawid Kmak + + ament_cmake + + joint_state_publisher + launch + launch_ros + robot_state_publisher + ros_components_description + xacro + + + ament_cmake + + diff --git a/lynx_description/urdf/body.urdf.xacro b/lynx_description/urdf/body.urdf.xacro new file mode 100644 index 00000000..c424ba01 --- /dev/null +++ b/lynx_description/urdf/body.urdf.xacro @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lynx_description/urdf/gazebo.urdf.xacro b/lynx_description/urdf/gazebo.urdf.xacro new file mode 100644 index 00000000..ce1a25c8 --- /dev/null +++ b/lynx_description/urdf/gazebo.urdf.xacro @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + ${ns}battery + 41.4 + 42.0 + -10.0 + ${battery_config['initial_charge_percentage']*battery_config['capacity']} + ${battery_config['capacity']} + 0.15 + 2.0 + true + ${battery_config['charging_time']} + 2.0 + + ${battery_config['power_load']/100.0} + ${battery_config['simulate_discharging']} + + + + + + + + + ${config_file} + + ${namespace} + drive_controller/cmd_vel_unstamped:=cmd_vel + drive_controller/odom:=odometry/wheels + drive_controller/transition_event:=drive_controller/_transition_event + 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 + imu_broadcaster/transition_event:=imu_broadcaster/_transition_event + joint_state_broadcaster/transition_event:=joint_state_broadcaster/_transition_event + + + + + + + + + ${mean} + ${stddev} + ${bias_mean} + ${bias_stddev} + ${precision} + + + + + + + + + + + + + + + + true + 50 + ${ns}imu/data_raw + false + false + imu_link + + + ENU + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 0 + 1.0 1.0 1.0 + 1.0 1.0 1.0 + 1 0 0 + 1.0 + + 1.0 + 2.0 + 0.4 + + + + + + diff --git a/lynx_description/urdf/lynx.urdf.xacro b/lynx_description/urdf/lynx.urdf.xacro new file mode 100644 index 00000000..8a78c117 --- /dev/null +++ b/lynx_description/urdf/lynx.urdf.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lynx_description/urdf/lynx_macro.urdf.xacro b/lynx_description/urdf/lynx_macro.urdf.xacro new file mode 100644 index 00000000..f835d42f --- /dev/null +++ b/lynx_description/urdf/lynx_macro.urdf.xacro @@ -0,0 +1,190 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + husarion_ugv_gazebo/EStopSystem + true + + + + husarion_ugv_hardware_interfaces/LynxSystem + + 1600 + + 30.08 + 0.75 + + + 0.11 + + + 3600.0 + + robot_can + 3 + 1 + 100 + + + 15 + 75 + + + 20.0 + + 5 + 5 + + + 2 + 2 + 2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + husarion_ugv_hardware_interfaces/PhidgetImuSensor + -1 + 0 + 8 + 1 + + + 0.00304 + 0.00151 + 0.0 + 0.0 + 0.0 + true + false + false + enu + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lynx_description/urdf/wheel.urdf.xacro b/lynx_description/urdf/wheel.urdf.xacro new file mode 100644 index 00000000..7003ab3e --- /dev/null +++ b/lynx_description/urdf/wheel.urdf.xacro @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1.0 + 0.0 + ${fdir} + + + + + + + + + + + diff --git a/panther/README.md b/panther/README.md deleted file mode 100644 index abc2f7d2..00000000 --- a/panther/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# panther - -ROS 2 Metapackage composing basic functionalities of the Husarion Panther robot with VCS Tool yaml files directing to external robot dependencies. diff --git a/panther_controller/CONFIGURATION.md b/panther_controller/CONFIGURATION.md deleted file mode 100644 index f541b872..00000000 --- a/panther_controller/CONFIGURATION.md +++ /dev/null @@ -1,9 +0,0 @@ -# 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_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py index 13a0b83d..1297f7c1 100644 --- a/panther_description/launch/load_urdf.launch.py +++ b/panther_description/launch/load_urdf.launch.py @@ -65,20 +65,20 @@ def generate_launch_description(): ), ) - wheel_type = LaunchConfiguration("wheel_type") # wheel_type is used by controller_config_path + wheel_type = LaunchConfiguration("wheel_type") controller_config_path = LaunchConfiguration("controller_config_path") declare_controller_config_path_arg = DeclareLaunchArgument( "controller_config_path", default_value=PathJoinSubstitution( [ - FindPackageShare("panther_controller"), + FindPackageShare("husarion_ugv_controller"), "config", PythonExpression(["'", wheel_type, "_controller.yaml'"]), ] ), description=( "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify" + " 'husarion_ugv_controller/config/{wheel_type}_controller.yaml'. You can also specify" " the path to your custom controller configuration file here. " ), ) @@ -98,6 +98,7 @@ def generate_launch_description(): choices=["True", "true", "False", "false"], ) + wheel_config_path = LaunchConfiguration("wheel_config_path") declare_wheel_config_path_arg = DeclareLaunchArgument( "wheel_config_path", default_value=PathJoinSubstitution( @@ -114,21 +115,24 @@ def generate_launch_description(): ), ) - wheel_config_path = LaunchConfiguration("wheel_config_path") declare_wheel_type_arg = DeclareLaunchArgument( "wheel_type", default_value="WH01", description=( - "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." + "Specify the wheel type. If the selected wheel type is not 'custom', " + "the 'wheel_config_path' and 'controller_config_path' arguments will be " + "automatically adjusted and can be omitted." ), choices=["WH01", "WH02", "WH04", "custom"], ) - panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0") # Get URDF via xacro + imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168") + imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028") + imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083") + imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14") + imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57") + imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0") robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -136,8 +140,6 @@ def generate_launch_description(): PathJoinSubstitution( [FindPackageShare("panther_description"), "urdf", "panther.urdf.xacro"] ), - " panther_version:=", - panther_version, " use_sim:=", use_sim, " wheel_config_file:=", @@ -147,9 +149,9 @@ def generate_launch_description(): " battery_config_file:=", battery_config_path, " imu_xyz:=", - f"\"{os.environ.get('PANTHER_IMU_LOCALIZATION_X', '0.168')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Y', '0.028')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Z', '0.083')}\"", + f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'", " imu_rpy:=", - f"\"{os.environ.get('PANTHER_IMU_ORIENTATION_R', '3.14')} {os.environ.get('PANTHER_IMU_ORIENTATION_P', '-1.57')} {os.environ.get('PANTHER_IMU_ORIENTATION_Y', '0.0')}\"", + f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'", " namespace:=", namespace, " components_config_path:=", diff --git a/panther_description/launch/overwrite_robot_description.launch.py b/panther_description/launch/overwrite_robot_description.launch.py new file mode 100644 index 00000000..a7f5cff9 --- /dev/null +++ b/panther_description/launch/overwrite_robot_description.launch.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python3 + +# Copyright 2020 ros2_control Development Team +# 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. + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import ( + Command, + EnvironmentVariable, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import SetParameter +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + battery_config_path = LaunchConfiguration("battery_config_path") + declare_battery_config_path_arg = DeclareLaunchArgument( + "battery_config_path", + description=( + "Path to the Ignition LinearBatteryPlugin configuration file. " + "This configuration is intended for use in simulations only." + ), + default_value="", + ) + + components_config_path = LaunchConfiguration("components_config_path") + declare_components_config_path_arg = DeclareLaunchArgument( + "components_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("panther_description"), "config", "components.yaml"] + ), + description=( + "Additional components configuration file. Components described in this file " + "are dynamically included in Panther's urdf." + "Panther options are described here " + "https://husarion.com/manuals/panther/panther-options/" + ), + ) + + wheel_type = LaunchConfiguration("wheel_type") + controller_config_path = LaunchConfiguration("controller_config_path") + declare_controller_config_path_arg = DeclareLaunchArgument( + "controller_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("husarion_ugv_controller"), + "config", + PythonExpression(["'", wheel_type, "_controller.yaml'"]), + ] + ), + description=( + "Path to controller configuration file. By default, it is located in" + " 'husarion_ugv_controller/config/{wheel_type}_controller.yaml'. You can also specify" + " the path to your custom controller configuration file here. " + ), + ) + + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Add namespace to all launched nodes.", + ) + + use_sim = LaunchConfiguration("use_sim") + declare_use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="False", + description="Whether simulation is used.", + choices=["True", "true", "False", "false"], + ) + + wheel_config_path = LaunchConfiguration("wheel_config_path") + declare_wheel_config_path_arg = DeclareLaunchArgument( + "wheel_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("panther_description"), + "config", + PythonExpression(["'", wheel_type, ".yaml'"]), + ] + ), + description=( + "Path to wheel configuration file. By default, it is located in " + "'panther_description/config/{wheel_type}.yaml'. You can also specify the path " + "to your custom wheel configuration file here. " + ), + ) + + declare_wheel_type_arg = DeclareLaunchArgument( + "wheel_type", + default_value="WH01", + description=( + "Specify the wheel type. If the selected wheel type is not 'custom', " + "the 'wheel_config_path' and 'controller_config_path' arguments will be " + "automatically adjusted and can be omitted." + ), + choices=["WH01", "WH02", "WH04", "custom"], + ) + + # Get URDF via xacro + imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168") + imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028") + imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083") + imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14") + imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57") + imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0") + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("panther_description"), "urdf", "panther.urdf.xacro"] + ), + " use_sim:=", + use_sim, + " wheel_config_file:=", + wheel_config_path, + " controller_config_file:=", + controller_config_path, + " battery_config_file:=", + battery_config_path, + " imu_xyz:=", + f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'", + " imu_rpy:=", + f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'", + " namespace:=", + namespace, + " components_config_path:=", + components_config_path, + ] + ) + + namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"]) + + set_robot_description = ExecuteProcess( + cmd=[ + "ros2", + "param", + "set", + [namespace_ext, "robot_state_publisher"], + "robot_description", + robot_description_content, + ], + output="screen", + ) + + actions = [ + declare_battery_config_path_arg, + declare_components_config_path_arg, + declare_wheel_type_arg, # wheel_type is used by controller_config_path + declare_controller_config_path_arg, + declare_namespace_arg, + declare_use_sim_arg, + declare_wheel_config_path_arg, + SetParameter(name="use_sim_time", value=use_sim), + set_robot_description, + ] + + return LaunchDescription(actions) diff --git a/panther_description/launch/rviz.launch.py b/panther_description/launch/rviz.launch.py new file mode 100644 index 00000000..b194341c --- /dev/null +++ b/panther_description/launch/rviz.launch.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 + +# Copyright 2020 ros2_control Development Team +# 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ReplaceString + + +def generate_launch_description(): + + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Add namespace to all launched nodes.", + ) + + rviz_config = LaunchConfiguration("rviz_config") + declare_rviz_config_arg = DeclareLaunchArgument( + "rviz_config", + default_value=PathJoinSubstitution( + [FindPackageShare("panther_description"), "rviz", "husarion_ugv.rviz"] + ), + description="RViz configuration file.", + ) + + use_sim = LaunchConfiguration("use_sim") + declare_use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="False", + description="Whether simulation is used.", + choices=["True", "true", "False", "false"], + ) + + ns_ext = PythonExpression(["'' if '", namespace, "' else '", namespace, "' + '/'"]) + + rviz_config = ReplaceString( + source_file=rviz_config, + replacements={"/": ns_ext, "": namespace}, + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + namespace=namespace, + arguments=["-d", rviz_config], + ) + + actions = [ + declare_namespace_arg, + declare_rviz_config_arg, + declare_use_sim_arg, + SetParameter(name="use_sim_time", value=use_sim), + rviz_node, + ] + + return LaunchDescription(actions) diff --git a/panther_description/package.xml b/panther_description/package.xml index 9b30cc77..3c0cdb72 100644 --- a/panther_description/package.xml +++ b/panther_description/package.xml @@ -20,8 +20,10 @@ joint_state_publisher launch launch_ros + nav2_common robot_state_publisher ros_components_description + rviz2 xacro diff --git a/panther_description/rviz/panther.rviz b/panther_description/rviz/husarion_ugv.rviz similarity index 52% rename from panther_description/rviz/panther.rviz rename to panther_description/rviz/husarion_ugv.rviz index 3fce6812..8285a047 100644 --- a/panther_description/rviz/panther.rviz +++ b/panther_description/rviz/husarion_ugv.rviz @@ -3,8 +3,7 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: - - /Global Options1 + Expanded: ~ Splitter Ratio: 0.5 Tree Height: 719 - Class: rviz_common/Selection @@ -28,107 +27,115 @@ Panels: Visualization Manager: Class: "" Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - body_link: - Alpha: 1 - Show Axes: false - Show Trail: false + - Class: rviz_common/Group + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: Value: true - cover_link: - Alpha: 1 - Show Axes: false - Show Trail: false - fl_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - front_bumper_link: - Alpha: 1 - Show Axes: false - Show Trail: false - front_light_link: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - rear_bumper_link: - Alpha: 1 - Show Axes: false - Show Trail: false - rear_light_link: - Alpha: 1 - Show Axes: false - Show Trail: false - rl_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rr_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false + Enabled: true + Name: Map + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: + Update Interval: 0 Value: true - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.05000000074505806 + Color: 255; 25; 0 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.05000000074505806 + Color: 255; 25; 0 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1000 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.20000000298023224 + Head Radius: 0.10000000149011612 + Shaft Length: 0.5 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: odometry/filtered + Value: false + Enabled: true + Name: Robot Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: odom + Fixed Frame: /odom Frame Rate: 30 Name: root Tools: @@ -148,14 +155,14 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /initialpose + Value: initialpose - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /goal_pose + Value: goal_pose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: @@ -163,7 +170,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /clicked_point + Value: clicked_point Transformation: Current: Class: rviz_default_plugins/TF @@ -171,25 +178,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 5.996953964233398 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 1.012673020362854 + Y: 0.033135633915662766 + Z: 0.347855806350708 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7853981852531433 + Pitch: 0.5903985500335693 Target Frame: Value: Orbit (rviz) - Yaw: 0.7853981852531433 + Yaw: 1.3703993558883667 Saved: ~ Window Geometry: Displays: @@ -197,7 +204,7 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005de0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -206,6 +213,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1850 - X: 1990 + Width: 1920 + X: 0 Y: 27 diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro index 29e3558f..40918200 100644 --- a/panther_description/urdf/gazebo.urdf.xacro +++ b/panther_description/urdf/gazebo.urdf.xacro @@ -37,12 +37,15 @@ ${config_file} ${namespace} + drive_controller/cmd_vel_unstamped:=cmd_vel + drive_controller/odom:=odometry/wheels + drive_controller/transition_event:=drive_controller/_transition_event 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 + imu_broadcaster/transition_event:=imu_broadcaster/_transition_event + joint_state_broadcaster/transition_event:=joint_state_broadcaster/_transition_event @@ -109,7 +112,7 @@ - + ${name} ${topic} ${namespace} diff --git a/panther_description/urdf/panther.urdf.xacro b/panther_description/urdf/panther.urdf.xacro index 25e4c754..a60b84e9 100644 --- a/panther_description/urdf/panther.urdf.xacro +++ b/panther_description/urdf/panther.urdf.xacro @@ -1,7 +1,6 @@ - @@ -10,7 +9,7 @@ + default="$(find husarion_ugv_controller)/config/WH01_controller.yaml" /> @@ -18,7 +17,6 @@ - panther_gazebo/GzPantherSystem + husarion_ugv_gazebo/EStopSystem true - - panther_hardware_interfaces/PantherSystem - - ${panther_version} + + husarion_ugv_hardware_interfaces/PantherSystem 1600 @@ -77,7 +74,7 @@ 3600.0 - panther_can + robot_can 3 1 2 @@ -143,10 +140,10 @@ - + - panther_hardware_interfaces/PantherImuSensor + husarion_ugv_hardware_interfaces/PhidgetImuSensor -1 0 8 diff --git a/panther_gazebo/launch/simulate_multiple_robots.launch.py b/panther_gazebo/launch/simulate_multiple_robots.launch.py deleted file mode 100644 index 9a9dc497..00000000 --- a/panther_gazebo/launch/simulate_multiple_robots.launch.py +++ /dev/null @@ -1,122 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, - PythonExpression, -) -from launch_ros.actions import Node, SetUseSimTime -from launch_ros.substitutions import FindPackageShare -from nav2_common.launch import ParseMultiRobotPose - - -def generate_launch_description(): - add_world_transform = LaunchConfiguration("add_world_transform") - declare_add_world_transform_arg = DeclareLaunchArgument( - "add_world_transform", - default_value="False", - description=( - "Adds a world frame that connects the tf trees of individual robots (useful when running" - " multiple robots)." - ), - choices=["True", "true", "False", "false"], - ) - - declare_robots_arg = DeclareLaunchArgument( - "robots", - default_value=[], - description=( - "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}'." - ), - ) - - robots_list = ParseMultiRobotPose("robots").value() - # If robots arg is empty, use default arguments from simulate_robot.launch.py - if len(robots_list) == 0: - robots_list = { - LaunchConfiguration("namespace", default=""): { - "x": LaunchConfiguration("x", default="0.0"), - "y": LaunchConfiguration("y", default="-2.0"), - "z": LaunchConfiguration("z", default="0.2"), - "roll": LaunchConfiguration("roll", default="0.0"), - "pitch": LaunchConfiguration("pitch", default="0.0"), - "yaw": LaunchConfiguration("yaw", default="0.0"), - } - } - else: - for robot_name, init_pose in robots_list.items(): - robots_list[robot_name] = {k: str(v) for k, v in init_pose.items()} - - simulate_robot = [] - for idx, robot_name in enumerate(robots_list): - init_pose = robots_list[robot_name] - x, y, z, roll, pitch, yaw = [value for value in init_pose.values()] - - spawn_robot_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("panther_gazebo"), "launch", "simulate_robot.launch.py"] - ) - ), - launch_arguments={ - "namespace": robot_name, - "x": x, - "y": y, - "z": z, - "roll": roll, - "pitch": pitch, - "yaw": yaw, - }.items(), - ) - - child_tf = PythonExpression( - ["'", robot_name, "' + '/odom' if '", robot_name, "' else 'odom'"] - ) - - world_transform = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_tf_publisher", - arguments=[x, y, z, roll, pitch, yaw, "world", child_tf], - namespace=robot_name, - emulate_tty=True, - condition=IfCondition(add_world_transform), - ) - - # Add small delay to prevent namespace overwriting - group = TimerAction( - period=5.0 * idx, - actions=[ - spawn_robot_launch, - world_transform, - ], - ) - simulate_robot.append(group) - - return LaunchDescription( - [ - declare_add_world_transform_arg, - declare_robots_arg, - SetUseSimTime(True), - *simulate_robot, - ] - ) diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt deleted file mode 100644 index 8713f699..00000000 --- a/panther_hardware_interfaces/CMakeLists.txt +++ /dev/null @@ -1,225 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) - -# Handle superbuild first -option(USE_SUPERBUILD "Whether or not a superbuild should be invoked" ON) - -if(USE_SUPERBUILD) - project(SUPERBUILD NONE) - include(cmake/SuperBuild.cmake) - return() -else() - project(panther_hardware_interfaces) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(PACKAGE_DEPENDENCIES - ament_cmake - controller_interface - diagnostic_updater - generate_parameter_library - geometry_msgs - hardware_interface - imu_filter_madgwick - panther_msgs - panther_utils - phidgets_api - PkgConfig - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_srvs - tf2_geometry_msgs - tf2_ros) - -foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) - find_package(${PACKAGE} REQUIRED) -endforeach() - -include_directories(include) - -generate_parameter_library(phidgets_spatial_parameters - config/phidgets_spatial_parameters.yaml) - -set(ENV{PKG_CONFIG_PATH} - "${CMAKE_INSTALL_PREFIX}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}") - -pkg_check_modules(LIBLELY_COAPP REQUIRED IMPORTED_TARGET liblely-coapp) -pkg_check_modules(LIBGPIOD REQUIRED IMPORTED_TARGET libgpiodcxx) - -add_library( - ${PROJECT_NAME} SHARED - src/panther_imu_sensor/panther_imu_sensor.cpp - src/panther_system/gpio/gpio_controller.cpp - src/panther_system/gpio/gpio_driver.cpp - src/panther_system/motors_controller/canopen_controller.cpp - src/panther_system/motors_controller/motors_controller.cpp - src/panther_system/motors_controller/roboteq_data_converters.cpp - src/panther_system/motors_controller/roboteq_driver.cpp - src/panther_system/motors_controller/roboteq_error_filter.cpp - src/panther_system/panther_system_e_stop.cpp - src/panther_system/panther_system_ros_interface.cpp - src/panther_system/panther_system.cpp - src/utils.cpp) -ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_DEPENDENCIES}) -target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP - PkgConfig::LIBGPIOD phidgets_spatial_parameters) - -target_compile_definitions(${PROJECT_NAME} - PRIVATE "PANTHER_HARDWARE_INTERFACES_BUILDING_DLL") - -pluginlib_export_plugin_description_file(hardware_interface - panther_hardware_interfaces.xml) - -install( - TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib) - -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(panther_utils REQUIRED) - - install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test) - - ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp src/utils.cpp) - - ament_add_gtest(${PROJECT_NAME}_test_panther_imu_sensor - test/panther_imu_sensor/test_panther_imu_sensor.cpp) - ament_target_dependencies( - ${PROJECT_NAME}_test_panther_imu_sensor hardware_interface rclcpp - panther_utils panther_msgs phidgets_api) - target_link_libraries(${PROJECT_NAME}_test_panther_imu_sensor ${PROJECT_NAME} - phidgets_spatial_parameters) - - ament_add_gtest( - ${PROJECT_NAME}_test_roboteq_error_filter - test/panther_system/motors_controller/test_roboteq_error_filter.cpp - src/panther_system/motors_controller/roboteq_error_filter.cpp) - - ament_add_gtest( - ${PROJECT_NAME}_test_roboteq_data_converters - test/panther_system/motors_controller/test_roboteq_data_converters.cpp - src/panther_system/motors_controller/roboteq_data_converters.cpp - src/utils.cpp) - target_include_directories( - ${PROJECT_NAME}_test_roboteq_data_converters - PUBLIC $ - $) - ament_target_dependencies(${PROJECT_NAME}_test_roboteq_data_converters - panther_msgs panther_utils) - target_link_libraries(${PROJECT_NAME}_test_roboteq_data_converters - PkgConfig::LIBLELY_COAPP) - - ament_add_gtest( - ${PROJECT_NAME}_test_canopen_controller - test/panther_system/motors_controller/test_canopen_controller.cpp - src/panther_system/motors_controller/canopen_controller.cpp - src/panther_system/motors_controller/roboteq_driver.cpp - src/utils.cpp) - target_include_directories( - ${PROJECT_NAME}_test_canopen_controller - PUBLIC $ - $) - ament_target_dependencies(${PROJECT_NAME}_test_canopen_controller rclcpp - panther_msgs panther_utils) - target_link_libraries(${PROJECT_NAME}_test_canopen_controller - PkgConfig::LIBLELY_COAPP) - - ament_add_gtest( - ${PROJECT_NAME}_test_roboteq_driver - test/panther_system/motors_controller/test_roboteq_driver.cpp - src/panther_system/motors_controller/canopen_controller.cpp - src/panther_system/motors_controller/roboteq_driver.cpp - src/utils.cpp) - target_include_directories( - ${PROJECT_NAME}_test_roboteq_driver - PUBLIC $ - $) - ament_target_dependencies(${PROJECT_NAME}_test_roboteq_driver rclcpp - panther_msgs panther_utils) - target_link_libraries(${PROJECT_NAME}_test_roboteq_driver - PkgConfig::LIBLELY_COAPP) - - ament_add_gtest( - ${PROJECT_NAME}_test_motors_controller - test/panther_system/motors_controller/test_motors_controller.cpp - src/panther_system/motors_controller/canopen_controller.cpp - src/panther_system/motors_controller/roboteq_driver.cpp - src/panther_system/motors_controller/roboteq_data_converters.cpp - src/panther_system/motors_controller/motors_controller.cpp - src/utils.cpp) - target_include_directories( - ${PROJECT_NAME}_test_motors_controller - PUBLIC $ - $) - ament_target_dependencies(${PROJECT_NAME}_test_motors_controller rclcpp - panther_msgs panther_utils) - target_link_libraries(${PROJECT_NAME}_test_motors_controller - PkgConfig::LIBLELY_COAPP) - - ament_add_gtest(${PROJECT_NAME}_test_gpiod_driver - test/panther_system/gpio/test_gpio_driver.cpp) - ament_target_dependencies(${PROJECT_NAME}_test_gpiod_driver panther_utils) - target_link_libraries(${PROJECT_NAME}_test_gpiod_driver ${PROJECT_NAME} - PkgConfig::LIBGPIOD) - - ament_add_gtest( - ${PROJECT_NAME}_test_gpiod_controller - test/panther_system/gpio/test_gpio_controller.cpp - src/panther_system/gpio/gpio_controller.cpp - src/panther_system/gpio/gpio_driver.cpp) - ament_target_dependencies(${PROJECT_NAME}_test_gpiod_controller panther_utils) - target_link_libraries(${PROJECT_NAME}_test_gpiod_controller - PkgConfig::LIBGPIOD) - - ament_add_gtest( - ${PROJECT_NAME}_test_panther_system_ros_interface - test/panther_system/test_panther_system_ros_interface.cpp - src/panther_system/panther_system_ros_interface.cpp - src/panther_system/motors_controller/roboteq_data_converters.cpp - src/utils.cpp - src/panther_system/gpio/gpio_controller.cpp - src/panther_system/gpio/gpio_driver.cpp) - target_include_directories( - ${PROJECT_NAME}_test_panther_system_ros_interface - PUBLIC $ - $) - ament_target_dependencies( - ${PROJECT_NAME}_test_panther_system_ros_interface - diagnostic_updater - rclcpp - panther_msgs - panther_utils - realtime_tools - std_srvs) - target_link_libraries(${PROJECT_NAME}_test_panther_system_ros_interface - PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD) - - ament_add_gtest(${PROJECT_NAME}_test_panther_system - test/panther_system/test_panther_system.cpp) - set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT - 120) - target_include_directories( - ${PROJECT_NAME}_test_panther_system - PUBLIC $ - $) - ament_target_dependencies( - ${PROJECT_NAME}_test_panther_system hardware_interface rclcpp panther_msgs - panther_utils) - target_link_libraries(${PROJECT_NAME}_test_panther_system - PkgConfig::LIBLELY_COAPP) - -endif() - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(${PACKAGE_DEPENDENCIES}) - -ament_package() diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp deleted file mode 100644 index e8e7c00a..00000000 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp +++ /dev/null @@ -1,148 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_MOTORS_CONTROLLER_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_MOTORS_CONTROLLER_HPP_ - -#include - -#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp" - -namespace panther_hardware_interfaces -{ - -/** - * @brief This class abstracts the usage of two Roboteq controllers. - * It uses canopen_controller for communication with Roboteq controllers, - * implements the activation procedure for controllers (resets script and sends initial 0 command), - * and provides methods to get data feedback and send commands. - * Data is converted between raw Roboteq formats and SI units using roboteq_data_converters. - */ -class MotorsController -{ -public: - MotorsController( - const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings); - - ~MotorsController() { Deinitialize(); } - - /** - * @brief Starts CAN communication and waits for boot to finish - * - * @exception std::runtime_error if boot fails - */ - void Initialize(); - - /** - * @brief Deinitialize can communication - */ - void Deinitialize(); - - /** - * @brief Activate procedure for Roboteq drivers - reset scripts and send 0 commands on both - * channels. Blocking function, takes around 2 seconds to finish - * - * @exception std::runtime_error if any procedure step fails - */ - void Activate(); - - /** - * @brief Updates current communication state with Roboteq drivers - * - * @exception std::runtime_error if CAN error was detected - */ - void UpdateCommunicationState(); - - /** - * @brief Updates current motors' state (position, velocity, current). - * - * @exception std::runtime_error if CAN error was detected - */ - void UpdateMotorsState(); - - /** - * @brief Updates current Roboteq driver state (flags, temperatures, voltage, battery current) - * - * @exception std::runtime_error if CAN error was detected - */ - void UpdateDriversState(); - - const RoboteqData & GetFrontData() { return front_data_; } - const RoboteqData & GetRearData() { return rear_data_; } - - /** - * @brief Write speed commands to motors - * - * @param speed_fl front left motor speed in rad/s - * @param speed_fr front right motor speed in rad/s - * @param speed_rl rear left motor speed in rad/s - * @param speed_rr rear right motor speed in rad/s - * - * @exception std::runtime_error if send command fails or CAN error was detected - */ - void SendSpeedCommands( - const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr); - - /** - * @brief Turns on Roboteq E-stop - * - * @exception std::runtime_error if any operation returns error - */ - void TurnOnEStop(); - - /** - * @brief Turns off Roboteq E-stop - * - * @exception std::runtime_error if any operation returns error - */ - void TurnOffEStop(); - - /** - * @brief Turns on Roboteq safety stop. To turn it off, it is necessary to send - * 0 commands to motors. - * - * @exception std::runtime_error if any operation returns error - */ - void TurnOnSafetyStop(); - - /** - * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq - * driver faults still exist, the error flag will remain active. - */ - inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); }; - -private: - void SetMotorsStates( - RoboteqData & data, const RoboteqMotorsStates & states, const timespec & current_time); - void SetDriverState( - RoboteqData & data, const RoboteqDriverState & state, const timespec & current_time); - - bool initialized_ = false; - - CANopenController canopen_controller_; - - RoboteqData front_data_; - RoboteqData rear_data_; - - RoboteqVelocityCommandConverter roboteq_vel_cmd_converter_; - - const std::chrono::milliseconds pdo_motor_states_timeout_ms_; - const std::chrono::milliseconds pdo_driver_state_timeout_ms_; -}; - -} // namespace panther_hardware_interfaces - -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_MOTORS_CONTROLLER_MOTORS_CONTROLLER_HPP_ diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp deleted file mode 100644 index 146bd909..00000000 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/panther_system_e_stop.hpp +++ /dev/null @@ -1,191 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_E_STOP_HPP_ -#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_E_STOP_HPP_ - -#include -#include -#include - -#include "panther_hardware_interfaces/panther_system/gpio/gpio_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_error_filter.hpp" - -namespace panther_hardware_interfaces -{ - -/** - * @class EStopInterface - * @brief Abstract base class defining the interface for emergency stop detailed implementations. - */ -class EStopInterface -{ -public: - EStopInterface( - std::shared_ptr gpio_controller, - std::shared_ptr roboteq_error_filter, - std::shared_ptr motors_controller, - std::shared_ptr motor_controller_write_mtx, - std::function zero_velocity_check) - : gpio_controller_(gpio_controller), - roboteq_error_filter_(roboteq_error_filter), - motors_controller_(motors_controller), - motor_controller_write_mtx_(motor_controller_write_mtx), - ZeroVelocityCheck(zero_velocity_check) {}; - - virtual ~EStopInterface() = default; - - virtual bool ReadEStopState() = 0; - virtual void TriggerEStop() = 0; - virtual void ResetEStop() = 0; - -protected: - std::shared_ptr gpio_controller_; - std::shared_ptr roboteq_error_filter_; - std::shared_ptr motors_controller_; - std::shared_ptr motor_controller_write_mtx_; - - std::function ZeroVelocityCheck; - - std::mutex e_stop_manipulation_mtx_; - std::atomic_bool e_stop_triggered_ = true; -}; - -/** - * @class EStopPTH12X - * @brief Implements the emergency stop for the PTH12X hardware variant. - */ -class EStopPTH12X : public EStopInterface -{ -public: - EStopPTH12X( - std::shared_ptr gpio_controller, - std::shared_ptr roboteq_error_filter, - std::shared_ptr motors_controller, - std::shared_ptr motor_controller_write_mtx, - std::function zero_velocity_check) - : EStopInterface( - gpio_controller, roboteq_error_filter, motors_controller, motor_controller_write_mtx, - zero_velocity_check) {}; - - virtual ~EStopPTH12X() override = default; - - /** - * @brief Checks the emergency stop state. - * - * 1. Check if ESTOP GPIO pin is not active. If is not it means that E-Stop is triggered by - * another device within the robot's system (e.g., Roboteq controller or Safety Board), - * disabling the software Watchdog is necessary to prevent an uncontrolled reset. - * 2. If there is a need, disable software Watchdog using - * GPIOControllerInterface::EStopTrigger method. - * 3. Return ESTOP GPIO pin state. - */ - bool ReadEStopState() override; - - /** - * @brief Triggers the emergency stop. - * - * 1. Interrupt the E-Stop resetting process if it is in progress. - * 2. Attempt to trigger the E-Stop using GPIO by disabling the software-controlled watchdog. - * 3. If successful, set e_stop_triggered_ to true; otherwise, throw a std::runtime_error - * exception. - * - * @throws std::runtime_error if triggering the E-stop using GPIO fails. - */ - void TriggerEStop() override; - - /** - * @brief Resets the emergency stop. - * - * 1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement - * after an E-stop state change. - * 2. Attempt to reset the E-Stop using GPIO by manipulating the ESTOP GPIO pin. This operation - * may take some time and can be interrupted by the E-Stop trigger process. - * 3. Set the clear_error flag to allow for clearing of Roboteq errors. - * 4. Confirm the E-Stop reset was successful with the ReadEStopState method. - * - * @throws EStopResetInterrupted if the E-stop reset operation was halted because the E-stop was - * triggered again. - * @throws std::runtime_error if an error occurs when trying to reset the E-stop using GPIO. - */ - void ResetEStop() override; -}; - -/** - * @class EStopPTH10X - * @brief Implements the emergency stop for the PTH10X hardware variant. In this robot - * version, only a software-based E-Stop is supported. There are no hardware components that - * implement E-Stop functionality. - */ -class EStopPTH10X : public EStopInterface -{ -public: - EStopPTH10X( - std::shared_ptr gpio_controller, - std::shared_ptr roboteq_error_filter, - std::shared_ptr motors_controller, - std::shared_ptr motor_controller_write_mtx, - std::function zero_velocity_check) - : EStopInterface( - gpio_controller, roboteq_error_filter, motors_controller, motor_controller_write_mtx, - zero_velocity_check) {}; - - virtual ~EStopPTH10X() override = default; - - /** - * @brief Checks the emergency stop state. - * - * 1. Verify if the main switch is in the STAGE2 position to confirm if the motors are powered - * up. - * 2. Check for any errors reported by the Roboteq controller. - * 3. If the E-Stop is not currently triggered, initiate an E-Stop if the motors are not powered - * up or if a driver error has occurred. - * 4. Return the actual state of the E-Stop. - * - * @return A boolean indicating whether the E-Stop is currently triggered `true` or not `false`. - */ - bool ReadEStopState() override; - - /** - * @brief Trigger the emergency stop state. - * - * 1. Send a command to the Roboteq controllers to enable the Safety Stop. - * Note: The Safety Stop is a specific state of the Roboteq controller, distinct from - * the E-Stop state of the Panther robot. - * 2. Update the e_stop_triggered_ flag to indicate that the E-Stop state has been triggered. - */ - void TriggerEStop() override; - - /** - * @brief Resets the emergency stop. - * - * 1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement - * after an E-stop state change. - * 2. Verify if the main switch is in the STAGE2 position to confirm if the motors are powered - * up. - * 3. Check for any errors reported by the Roboteq controller. - * 4. Set the clear_error flag to allow for clearing of Roboteq errors. - * 5. Confirm the E-Stop reset was successful with the ReadEStopState method. - * 6. Update the e_stop_triggered_ flag to indicate that the E-Stop state has been triggered. - * - * @throws std::runtime_error if the E-stop reset operation was halted because the E-stop was - * triggered again, motors are not powered or motor controller is in an error state. - */ - void ResetEStop() override; -}; - -} // namespace panther_hardware_interfaces - -#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_PANTHER_SYSTEM_E_STOP_HPP_ diff --git a/panther_hardware_interfaces/panther_hardware_interfaces.xml b/panther_hardware_interfaces/panther_hardware_interfaces.xml deleted file mode 100644 index 8462de75..00000000 --- a/panther_hardware_interfaces/panther_hardware_interfaces.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - Hardware system controller for Panther's Roboteq motor controller - - - - - - Hardware IMU sensor for Panther's Phidget Spatial Inertial Measurement Unit - - - diff --git a/panther_hardware_interfaces/src/panther_system/motors_controller/motors_controller.cpp b/panther_hardware_interfaces/src/panther_system/motors_controller/motors_controller.cpp deleted file mode 100644 index e260fa86..00000000 --- a/panther_hardware_interfaces/src/panther_system/motors_controller/motors_controller.cpp +++ /dev/null @@ -1,254 +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_hardware_interfaces/panther_system/motors_controller/motors_controller.hpp" - -#include -#include -#include -#include - -#include "lely/util/chrono.hpp" - -#include "panther_hardware_interfaces/panther_system/motors_controller/canopen_controller.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_data_converters.hpp" -#include "panther_hardware_interfaces/panther_system/motors_controller/roboteq_driver.hpp" - -namespace panther_hardware_interfaces -{ - -MotorsController::MotorsController( - const CANopenSettings & canopen_settings, const DrivetrainSettings & drivetrain_settings) -: canopen_controller_(canopen_settings), - front_data_(drivetrain_settings), - rear_data_(drivetrain_settings), - roboteq_vel_cmd_converter_(drivetrain_settings), - pdo_motor_states_timeout_ms_(canopen_settings.pdo_motor_states_timeout_ms), - pdo_driver_state_timeout_ms_(canopen_settings.pdo_driver_state_timeout_ms) -{ -} - -void MotorsController::Initialize() -{ - if (initialized_) { - return; - } - - try { - canopen_controller_.Initialize(); - } catch (const std::runtime_error & e) { - throw e; - } - - initialized_ = true; -} - -void MotorsController::Deinitialize() -{ - canopen_controller_.Deinitialize(); - initialized_ = false; -} - -void MotorsController::Activate() -{ - // Activation procedure - it is necessary to first reset scripts, wait for a bit (1 second) - // and then send 0 commands for some time (also 1 second) - - try { - canopen_controller_.GetFrontDriver()->ResetRoboteqScript(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Front driver reset Roboteq script exception: " + std::string(e.what())); - } - - try { - canopen_controller_.GetRearDriver()->ResetRoboteqScript(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Rear driver reset Roboteq script exception: " + std::string(e.what())); - } - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - try { - canopen_controller_.GetFrontDriver()->SendRoboteqCmd(0, 0); - } catch (const std::runtime_error & e) { - throw std::runtime_error("Front driver send 0 command exception: " + std::string(e.what())); - } - - try { - canopen_controller_.GetRearDriver()->SendRoboteqCmd(0, 0); - } catch (const std::runtime_error & e) { - throw std::runtime_error("Rear driver send 0 command exception: " + std::string(e.what())); - } - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); -} - -void MotorsController::UpdateCommunicationState() -{ - front_data_.SetCANError(canopen_controller_.GetFrontDriver()->IsCANError()); - rear_data_.SetCANError(canopen_controller_.GetRearDriver()->IsCANError()); - - front_data_.SetHeartbeatTimeout(canopen_controller_.GetFrontDriver()->IsHeartbeatTimeout()); - rear_data_.SetHeartbeatTimeout(canopen_controller_.GetRearDriver()->IsHeartbeatTimeout()); -} - -void MotorsController::UpdateMotorsState() -{ - timespec current_time; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - - SetMotorsStates( - front_data_, canopen_controller_.GetFrontDriver()->ReadRoboteqMotorsStates(), current_time); - SetMotorsStates( - rear_data_, canopen_controller_.GetRearDriver()->ReadRoboteqMotorsStates(), current_time); - - UpdateCommunicationState(); - - if (front_data_.IsCANError() || rear_data_.IsCANError()) { - throw std::runtime_error("CAN error."); - } - - if (front_data_.IsHeartbeatTimeout() || rear_data_.IsHeartbeatTimeout()) { - throw std::runtime_error("Motor controller heartbeat timeout."); - } -} - -void MotorsController::UpdateDriversState() -{ - timespec current_time; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - - SetDriverState( - front_data_, canopen_controller_.GetFrontDriver()->ReadRoboteqDriverState(), current_time); - SetDriverState( - rear_data_, canopen_controller_.GetRearDriver()->ReadRoboteqDriverState(), current_time); - - UpdateCommunicationState(); - - if (front_data_.IsCANError() || rear_data_.IsCANError()) { - throw std::runtime_error("CAN error."); - } - - if (front_data_.IsHeartbeatTimeout() || rear_data_.IsHeartbeatTimeout()) { - throw std::runtime_error("Motor controller heartbeat timeout."); - } -} - -void MotorsController::SendSpeedCommands( - const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr) -{ - // Channel 1 - right motor, Channel 2 - left motor - try { - canopen_controller_.GetFrontDriver()->SendRoboteqCmd( - roboteq_vel_cmd_converter_.Convert(speed_fr), roboteq_vel_cmd_converter_.Convert(speed_fl)); - } catch (const std::runtime_error & e) { - throw std::runtime_error("Front driver send Roboteq cmd failed: " + std::string(e.what())); - } - try { - canopen_controller_.GetRearDriver()->SendRoboteqCmd( - roboteq_vel_cmd_converter_.Convert(speed_rr), roboteq_vel_cmd_converter_.Convert(speed_rl)); - } catch (const std::runtime_error & e) { - throw std::runtime_error("Rear driver send Roboteq cmd failed: " + std::string(e.what())); - } - - if (canopen_controller_.GetFrontDriver()->IsCANError()) { - throw std::runtime_error( - "CAN error detected on the front driver when trying to write speed commands."); - } - if (canopen_controller_.GetRearDriver()->IsCANError()) { - throw std::runtime_error( - "CAN error detected on the rear driver when trying to write speed commands."); - } -} - -void MotorsController::TurnOnEStop() -{ - try { - canopen_controller_.GetFrontDriver()->TurnOnEStop(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Failed to turn on E-stop on the front driver: " + std::string(e.what())); - } - try { - canopen_controller_.GetRearDriver()->TurnOnEStop(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Failed to turn on E-stop on the rear driver: " + std::string(e.what())); - } -} - -void MotorsController::TurnOffEStop() -{ - try { - canopen_controller_.GetFrontDriver()->TurnOffEStop(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Failed to turn off E-stop on the front driver: " + std::string(e.what())); - } - try { - canopen_controller_.GetRearDriver()->TurnOffEStop(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Failed to turn off E-stop on the rear driver: " + std::string(e.what())); - } -} - -void MotorsController::TurnOnSafetyStop() -{ - try { - canopen_controller_.GetFrontDriver()->TurnOnSafetyStopChannel1(); - canopen_controller_.GetFrontDriver()->TurnOnSafetyStopChannel2(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Failed to turn on safety stop on the front driver: " + std::string(e.what())); - } - try { - canopen_controller_.GetRearDriver()->TurnOnSafetyStopChannel1(); - canopen_controller_.GetRearDriver()->TurnOnSafetyStopChannel2(); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Failed to turn on safety stop on the rear driver: " + std::string(e.what())); - } -} - -void MotorsController::SetMotorsStates( - RoboteqData & data, const RoboteqMotorsStates & states, const timespec & current_time) -{ - bool data_timed_out = - (lely::util::from_timespec(current_time) - lely::util::from_timespec(states.pos_timestamp) > - pdo_motor_states_timeout_ms_) || - (lely::util::from_timespec(current_time) - - lely::util::from_timespec(states.vel_current_timestamp) > - pdo_motor_states_timeout_ms_); - - // Channel 1 - right, Channel 2 - left - data.SetMotorsStates(states.motor_2, states.motor_1, data_timed_out); -} - -void MotorsController::SetDriverState( - RoboteqData & data, const RoboteqDriverState & state, const timespec & current_time) -{ - bool data_timed_out = (lely::util::from_timespec(current_time) - - lely::util::from_timespec(state.flags_current_timestamp) > - pdo_driver_state_timeout_ms_) || - (lely::util::from_timespec(current_time) - - lely::util::from_timespec(state.voltages_temps_timestamp) > - pdo_driver_state_timeout_ms_); - - data.SetDriverState(state, data_timed_out); -} - -} // namespace panther_hardware_interfaces diff --git a/panther_hardware_interfaces/src/panther_system/panther_system.cpp b/panther_hardware_interfaces/src/panther_system/panther_system.cpp deleted file mode 100644 index 05c4c6e9..00000000 --- a/panther_hardware_interfaces/src/panther_system/panther_system.cpp +++ /dev/null @@ -1,752 +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_hardware_interfaces/panther_system/panther_system.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "diagnostic_updater/diagnostic_status_wrapper.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/logging.hpp" - -#include "panther_hardware_interfaces/utils.hpp" -#include "panther_utils/diagnostics.hpp" - -namespace panther_hardware_interfaces -{ - -CallbackReturn PantherSystem::on_init(const hardware_interface::HardwareInfo & hardware_info) -{ - if (hardware_interface::SystemInterface::on_init(hardware_info) != CallbackReturn::SUCCESS) { - return CallbackReturn::ERROR; - } - - try { - CheckJointSize(); - SortAndCheckJointNames(); - SetInitialValues(); - CheckInterfaces(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "An exception occurred while initializing: " << e.what()); - return CallbackReturn::ERROR; - } - - try { - ReadPantherVersion(); - ReadDrivetrainSettings(); - ReadCANopenSettings(); - ReadInitializationActivationAttempts(); - ReadParametersAndCreateRoboteqErrorFilter(); - ReadDriverStatesUpdateFrequency(); - } catch (const std::invalid_argument & e) { - RCLCPP_ERROR_STREAM( - logger_, "An exception occurred while reading the parameters: " << e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State &) -{ - RCLCPP_DEBUG_STREAM( - logger_, "Creating system entities for the Panther version: " << panther_version_); - - try { - ConfigureGPIOController(); - ConfigureMotorsController(); - ConfigureEStop(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM( - logger_, "Failed to initialize GPIO, Motors, or E-Stop controllers. Error: " << e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_cleanup(const rclcpp_lifecycle::State &) -{ - motors_controller_->Deinitialize(); - motors_controller_.reset(); - - gpio_controller_.reset(); - e_stop_.reset(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &) -{ - hw_commands_velocities_.fill(0.0); - hw_states_positions_.fill(0.0); - hw_states_velocities_.fill(0.0); - hw_states_efforts_.fill(0.0); - - if (!OperationWithAttempts( - std::bind(&MotorsController::Activate, motors_controller_), - max_roboteq_activation_attempts_)) { - RCLCPP_ERROR_STREAM( - logger_, "Failed to activate Panther System: Couldn't activate MotorsController in " - << max_roboteq_activation_attempts_ << " attempts."); - return CallbackReturn::ERROR; - } - - panther_system_ros_interface_ = - std::make_unique("hardware_controller"); - - panther_system_ros_interface_->AddService>( - "~/fan_enable", - std::bind(&GPIOControllerInterface::FanEnable, gpio_controller_, std::placeholders::_1)); - panther_system_ros_interface_->AddService>( - "~/aux_power_enable", - std::bind(&GPIOControllerInterface::AUXPowerEnable, gpio_controller_, std::placeholders::_1)); - panther_system_ros_interface_->AddService>( - "~/digital_power_enable", - std::bind( - &GPIOControllerInterface::DigitalPowerEnable, gpio_controller_, std::placeholders::_1)); - panther_system_ros_interface_->AddService>( - "~/charger_enable", - std::bind(&GPIOControllerInterface::ChargerEnable, gpio_controller_, std::placeholders::_1)); - panther_system_ros_interface_->AddService>( - "~/led_control_enable", - std::bind(&GPIOControllerInterface::LEDControlEnable, gpio_controller_, std::placeholders::_1)); - panther_system_ros_interface_->AddService>( - "~/motor_power_enable", - std::bind(&PantherSystem::MotorsPowerEnable, this, std::placeholders::_1)); - - panther_system_ros_interface_->AddService>( - "~/e_stop_trigger", std::bind(&EStopInterface::TriggerEStop, e_stop_), 1, - rclcpp::CallbackGroupType::MutuallyExclusive); - - auto e_stop_reset_qos = rmw_qos_profile_services_default; - e_stop_reset_qos.depth = 1; - panther_system_ros_interface_->AddService>( - "~/e_stop_reset", std::bind(&EStopInterface::ResetEStop, e_stop_), 2, - rclcpp::CallbackGroupType::MutuallyExclusive, e_stop_reset_qos); - - panther_system_ros_interface_->AddDiagnosticTask( - std::string("system errors"), this, &PantherSystem::DiagnoseErrors); - - panther_system_ros_interface_->AddDiagnosticTask( - std::string("system status"), this, &PantherSystem::DiagnoseStatus); - - gpio_controller_->RegisterGPIOEventCallback( - [this](const auto & state) { panther_system_ros_interface_->PublishIOState(state); }); - - const auto io_state = gpio_controller_->QueryControlInterfaceIOStates(); - panther_system_ros_interface_->InitializeAndPublishIOStateMsg(io_state); - - panther_system_ros_interface_->PublishEStopStateMsg(e_stop_->ReadEStopState()); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_deactivate(const rclcpp_lifecycle::State &) -{ - try { - e_stop_->TriggerEStop(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "Deactivation failed: " << e.what()); - return CallbackReturn::ERROR; - } - - panther_system_ros_interface_.reset(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_shutdown(const rclcpp_lifecycle::State &) -{ - try { - e_stop_->TriggerEStop(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "Shutdown failed: " << e.what()); - return CallbackReturn::ERROR; - } - - motors_controller_->Deinitialize(); - motors_controller_.reset(); - - gpio_controller_.reset(); - - panther_system_ros_interface_.reset(); - e_stop_.reset(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PantherSystem::on_error(const rclcpp_lifecycle::State &) -{ - try { - e_stop_->TriggerEStop(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "Handling error failed: " << e.what()); - return CallbackReturn::ERROR; - } - - panther_system_ros_interface_->BroadcastOnDiagnosticTasks( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, - "An error has occurred during a node state transition."); - - panther_system_ros_interface_.reset(); - - motors_controller_->Deinitialize(); - motors_controller_.reset(); - - gpio_controller_.reset(); - e_stop_.reset(); - - return CallbackReturn::SUCCESS; -} - -std::vector PantherSystem::export_state_interfaces() -{ - std::vector state_interfaces; - for (std::size_t i = 0; i < kJointsSize; i++) { - state_interfaces.emplace_back(StateInterface( - joints_names_sorted_[i], hardware_interface::HW_IF_POSITION, &hw_states_positions_[i])); - state_interfaces.emplace_back(StateInterface( - joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i])); - state_interfaces.emplace_back(StateInterface( - joints_names_sorted_[i], hardware_interface::HW_IF_EFFORT, &hw_states_efforts_[i])); - } - - return state_interfaces; -} - -std::vector PantherSystem::export_command_interfaces() -{ - std::vector command_interfaces; - for (std::size_t i = 0; i < kJointsSize; i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joints_names_sorted_[i], hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i])); - } - - return command_interfaces; -} - -return_type PantherSystem::read(const rclcpp::Time & time, const rclcpp::Duration & /* period */) -{ - UpdateMotorsState(); - - if (time >= next_driver_state_update_time_) { - UpdateDriverState(); - UpdateDriverStateMsg(); - panther_system_ros_interface_->PublishDriverState(); - next_driver_state_update_time_ = time + driver_states_update_period_; - } - - UpdateEStopState(); - - return return_type::OK; -} - -return_type PantherSystem::write( - const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) -{ - const bool e_stop = e_stop_->ReadEStopState(); - - if (!e_stop) { - HandlePDOWriteOperation([this] { - motors_controller_->SendSpeedCommands( - hw_commands_velocities_[0], hw_commands_velocities_[1], hw_commands_velocities_[2], - hw_commands_velocities_[3]); - }); - } - - return return_type::OK; -} - -void PantherSystem::CheckJointSize() const -{ - if (info_.joints.size() != kJointsSize) { - throw std::runtime_error( - "Wrong number of joints defined: " + std::to_string(info_.joints.size()) + ", " + - std::to_string(kJointsSize) + "expected."); - } -} - -void PantherSystem::SortAndCheckJointNames() -{ - // Sort joints names - later hw_states and hw_commands are accessed by static indexes, so it - // is necessary to make sure that joints are in specific order and order of definitions in URDF - // doesn't matter - for (std::size_t i = 0; i < kJointsSize; i++) { - std::size_t match_count = 0; - - for (std::size_t j = 0; j < kJointsSize; j++) { - if (CheckIfJointNameContainValidSequence(info_.joints[j].name, joint_order_[i])) { - joints_names_sorted_[i] = info_.joints[j].name; - ++match_count; - } - } - - if (match_count != 1) { - throw std::runtime_error( - "There should be exactly one joint containing " + joint_order_[i] + ", " + - std::to_string(match_count) + " found."); - } - } -} - -void PantherSystem::SetInitialValues() -{ - // It isn't safe to set command to NaN - sometimes it could be interpreted as Inf (although it - // shouldn't). In case of velocity, I think that setting the initial value to 0.0 is the best - // option. - hw_commands_velocities_.fill(0.0); - - hw_states_positions_.fill(std::numeric_limits::quiet_NaN()); - hw_states_velocities_.fill(std::numeric_limits::quiet_NaN()); - hw_states_efforts_.fill(std::numeric_limits::quiet_NaN()); -} - -void PantherSystem::CheckInterfaces() const -{ - for (const hardware_interface::ComponentInfo & joint : info_.joints) { - // Commands - if (joint.command_interfaces.size() != 1) { - throw std::runtime_error( - "Joint " + joint.name + " has " + std::to_string(joint.command_interfaces.size()) + - " command interfaces. 1 expected."); - } - - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { - throw std::runtime_error( - "Joint " + joint.name + " has " + joint.command_interfaces[0].name + - " command interface. " + hardware_interface::HW_IF_VELOCITY + " expected."); - } - - // States - if (joint.state_interfaces.size() != 3) { - throw std::runtime_error( - "Joint " + joint.name + " has " + std::to_string(joint.state_interfaces.size()) + - " state " + (joint.state_interfaces.size() == 1 ? "interface." : "interfaces.") + - " 3 expected."); - } - - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - throw std::runtime_error( - "Joint " + joint.name + " has " + joint.state_interfaces[0].name + - " as first state interface. " + hardware_interface::HW_IF_POSITION + " expected."); - } - - if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { - throw std::runtime_error( - "Joint " + joint.name + " has " + joint.state_interfaces[1].name + - " as second state interface. " + hardware_interface::HW_IF_VELOCITY + " expected."); - } - - if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { - throw std::runtime_error( - "Joint " + joint.name + " has " + joint.state_interfaces[2].name + - " as third state interface. " + hardware_interface::HW_IF_EFFORT + " expected."); - } - } -} - -void PantherSystem::ReadPantherVersion() -{ - panther_version_ = std::stof(info_.hardware_parameters["panther_version"]); -} - -void PantherSystem::ReadDrivetrainSettings() -{ - drivetrain_settings_.motor_torque_constant = - std::stof(info_.hardware_parameters["motor_torque_constant"]); - drivetrain_settings_.gear_ratio = std::stof(info_.hardware_parameters["gear_ratio"]); - drivetrain_settings_.gearbox_efficiency = - std::stof(info_.hardware_parameters["gearbox_efficiency"]); - drivetrain_settings_.encoder_resolution = - std::stof(info_.hardware_parameters["encoder_resolution"]); - drivetrain_settings_.max_rpm_motor_speed = - std::stof(info_.hardware_parameters["max_rpm_motor_speed"]); -} - -void PantherSystem::ReadCANopenSettings() -{ - canopen_settings_.can_interface_name = info_.hardware_parameters["can_interface_name"]; - canopen_settings_.master_can_id = std::stoi(info_.hardware_parameters["master_can_id"]); - canopen_settings_.front_driver_can_id = - std::stoi(info_.hardware_parameters["front_driver_can_id"]); - canopen_settings_.rear_driver_can_id = std::stoi(info_.hardware_parameters["rear_driver_can_id"]); - canopen_settings_.pdo_motor_states_timeout_ms = - std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_motor_states_timeout_ms"])); - canopen_settings_.pdo_driver_state_timeout_ms = - std::chrono::milliseconds(std::stoi(info_.hardware_parameters["pdo_driver_state_timeout_ms"])); - canopen_settings_.sdo_operation_timeout_ms = - std::chrono::milliseconds(std::stoi(info_.hardware_parameters["sdo_operation_timeout_ms"])); -} - -void PantherSystem::ReadInitializationActivationAttempts() -{ - max_roboteq_initialization_attempts_ = - std::stoi(info_.hardware_parameters["max_roboteq_initialization_attempts"]); - max_roboteq_activation_attempts_ = - std::stoi(info_.hardware_parameters["max_roboteq_activation_attempts"]); -} - -void PantherSystem::ReadParametersAndCreateRoboteqErrorFilter() -{ - const unsigned max_write_pdo_cmds_errors_count = - std::stoi(info_.hardware_parameters["max_write_pdo_cmds_errors_count"]); - const unsigned max_read_pdo_motor_states_errors_count = - std::stoi(info_.hardware_parameters["max_read_pdo_motor_states_errors_count"]); - const unsigned max_read_pdo_driver_state_errors_count = - std::stoi(info_.hardware_parameters["max_read_pdo_driver_state_errors_count"]); - - roboteq_error_filter_ = std::make_shared( - max_write_pdo_cmds_errors_count, max_read_pdo_motor_states_errors_count, - max_read_pdo_driver_state_errors_count, 1); -} - -void PantherSystem::ReadDriverStatesUpdateFrequency() -{ - const float driver_states_update_frequency = - std::stof(info_.hardware_parameters["driver_states_update_frequency"]); - driver_states_update_period_ = - rclcpp::Duration::from_seconds(1.0f / driver_states_update_frequency); -} - -void PantherSystem::ConfigureGPIOController() -{ - if (IsPantherVersionAtLeast(1.2)) { - gpio_controller_ = std::make_shared(); - } else { - gpio_controller_ = std::make_shared(); - } - - gpio_controller_->Start(); - - RCLCPP_INFO(logger_, "Successfully configured GPIO controller."); -} - -void PantherSystem::ConfigureMotorsController() -{ - motor_controller_write_mtx_ = std::make_shared(); - motors_controller_ = std::make_shared(canopen_settings_, drivetrain_settings_); - - if (!OperationWithAttempts( - std::bind(&MotorsController::Initialize, motors_controller_), - max_roboteq_initialization_attempts_, - std::bind(&MotorsController::Deinitialize, motors_controller_))) { - throw std::runtime_error("Roboteq drivers initialization failed."); - } - - RCLCPP_INFO(logger_, "Successfully configured motors controller"); -} - -void PantherSystem::ConfigureEStop() -{ - if ( - !gpio_controller_ || !roboteq_error_filter_ || !motors_controller_ || - !motor_controller_write_mtx_) { - throw std::runtime_error("Failed to configure E-Stop, make sure to setup entities first."); - } - - if (IsPantherVersionAtLeast(1.2f)) { - e_stop_ = std::make_shared( - gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_, - std::bind(&PantherSystem::AreVelocityCommandsNearZero, this)); - } else { - e_stop_ = std::make_shared( - gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_, - std::bind(&PantherSystem::AreVelocityCommandsNearZero, this)); - } - - RCLCPP_INFO(logger_, "Successfully configured E-Stop"); -} - -void PantherSystem::UpdateMotorsState() -{ - try { - motors_controller_->UpdateMotorsState(); - UpdateHwStates(); - UpdateMotorsStateDataTimedOut(); - } catch (const std::runtime_error & e) { - roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); - - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, steady_clock_, 5000, - "An exception occurred while updating motors states: " << e.what()); - } -} - -void PantherSystem::UpdateDriverState() -{ - try { - motors_controller_->UpdateDriversState(); - UpdateFlagErrors(); - UpdateDriverStateDataTimedOut(); - } catch (const std::runtime_error & e) { - roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); - - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, steady_clock_, 5000, - "An exception occurred while updating drivers states: " << e.what()); - } -} - -void PantherSystem::UpdateEStopState() -{ - if ( - motors_controller_->GetFrontData().IsHeartbeatTimeout() || - motors_controller_->GetRearData().IsHeartbeatTimeout()) { - e_stop_->TriggerEStop(); - } - - const bool e_stop = e_stop_->ReadEStopState(); - panther_system_ros_interface_->PublishEStopStateIfChanged(e_stop); -} - -void PantherSystem::UpdateHwStates() -{ - const auto front = motors_controller_->GetFrontData(); - const auto rear = motors_controller_->GetRearData(); - - const auto fl = front.GetLeftMotorState(); - const auto fr = front.GetRightMotorState(); - const auto rl = rear.GetLeftMotorState(); - const auto rr = rear.GetRightMotorState(); - - hw_states_positions_[0] = fl.GetPosition(); - hw_states_positions_[1] = fr.GetPosition(); - hw_states_positions_[2] = rl.GetPosition(); - hw_states_positions_[3] = rr.GetPosition(); - - hw_states_velocities_[0] = fl.GetVelocity(); - hw_states_velocities_[1] = fr.GetVelocity(); - hw_states_velocities_[2] = rl.GetVelocity(); - hw_states_velocities_[3] = rr.GetVelocity(); - - hw_states_efforts_[0] = fl.GetTorque(); - hw_states_efforts_[1] = fr.GetTorque(); - hw_states_efforts_[2] = rl.GetTorque(); - hw_states_efforts_[3] = rr.GetTorque(); -} - -void PantherSystem::UpdateMotorsStateDataTimedOut() -{ - if ( - motors_controller_->GetFrontData().IsMotorStatesDataTimedOut() || - motors_controller_->GetRearData().IsMotorStatesDataTimedOut()) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, steady_clock_, 1000, - (motors_controller_->GetFrontData().IsMotorStatesDataTimedOut() ? "Front " : "") - << (motors_controller_->GetRearData().IsMotorStatesDataTimedOut() ? "Rear " : "") - << "PDO motor state data timeout."); - roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, true); - } else { - roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_MOTOR_STATES, false); - } -} - -void PantherSystem::UpdateDriverStateMsg() -{ - panther_system_ros_interface_->UpdateMsgDriversStates( - motors_controller_->GetFrontData().GetDriverState(), - motors_controller_->GetRearData().GetDriverState()); - - panther_system_ros_interface_->UpdateMsgErrorFlags( - motors_controller_->GetFrontData(), motors_controller_->GetRearData()); - - CANErrors can_errors; - can_errors.error = roboteq_error_filter_->IsError(); - can_errors.write_pdo_cmds_error = roboteq_error_filter_->IsError(ErrorsFilterIds::WRITE_PDO_CMDS); - can_errors.read_pdo_motor_states_error = - roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_MOTOR_STATES); - can_errors.read_pdo_driver_state_error = - roboteq_error_filter_->IsError(ErrorsFilterIds::READ_PDO_DRIVER_STATE); - - can_errors.front_motor_states_data_timed_out = - motors_controller_->GetFrontData().IsMotorStatesDataTimedOut(); - can_errors.rear_motor_states_data_timed_out = - motors_controller_->GetRearData().IsMotorStatesDataTimedOut(); - - can_errors.front_driver_state_data_timed_out = - motors_controller_->GetFrontData().IsDriverStateDataTimedOut(); - can_errors.rear_driver_state_data_timed_out = - motors_controller_->GetRearData().IsDriverStateDataTimedOut(); - - can_errors.front_can_error = motors_controller_->GetFrontData().IsCANError(); - can_errors.rear_can_error = motors_controller_->GetRearData().IsCANError(); - - can_errors.front_heartbeat_timeout = motors_controller_->GetFrontData().IsHeartbeatTimeout(); - can_errors.rear_heartbeat_timeout = motors_controller_->GetRearData().IsHeartbeatTimeout(); - - panther_system_ros_interface_->UpdateMsgErrors(can_errors); -} - -void PantherSystem::UpdateFlagErrors() -{ - if ( - motors_controller_->GetFrontData().IsFlagError() || - motors_controller_->GetRearData().IsFlagError()) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, steady_clock_, 1000, - "Error state on one of the drivers:\n" - << "\tFront: " << motors_controller_->GetFrontData().GetFlagErrorLog() - << "\tRear: " << motors_controller_->GetRearData().GetFlagErrorLog()); - roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, true); - - HandlePDOWriteOperation([this] { motors_controller_->AttemptErrorFlagResetWithZeroSpeed(); }); - } else { - roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); - } -} - -void PantherSystem::UpdateDriverStateDataTimedOut() -{ - if ( - motors_controller_->GetFrontData().IsDriverStateDataTimedOut() || - motors_controller_->GetRearData().IsDriverStateDataTimedOut()) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, steady_clock_, 1000, - (motors_controller_->GetFrontData().IsDriverStateDataTimedOut() ? "Front " : "") - << (motors_controller_->GetRearData().IsDriverStateDataTimedOut() ? "Rear " : "") - << "PDO driver state timeout."); - roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, true); - } else { - roboteq_error_filter_->UpdateError(ErrorsFilterIds::READ_PDO_DRIVER_STATE, false); - } -} - -void PantherSystem::HandlePDOWriteOperation(std::function pdo_write_operation) -{ - try { - { - std::unique_lock motor_controller_write_lck( - *motor_controller_write_mtx_, std::defer_lock); - if (!motor_controller_write_lck.try_lock()) { - throw std::runtime_error( - "Can't acquire mutex for writing commands - E-stop is being triggered."); - } - pdo_write_operation(); - } - - roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, false); - } catch (const std::runtime_error & e) { - RCLCPP_WARN_STREAM(logger_, "An exception occurred while writing commands: " << e.what()); - roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, true); - } -} - -bool PantherSystem::AreVelocityCommandsNearZero() -{ - for (const auto & cmd : hw_commands_velocities_) { - if (std::abs(cmd) > std::numeric_limits::epsilon()) { - return false; - } - } - return true; -} - -bool PantherSystem::IsPantherVersionAtLeast(const float version) -{ - return panther_version_ >= version - std::numeric_limits::epsilon(); -} - -void PantherSystem::MotorsPowerEnable(const bool enable) -{ - try { - { - std::lock_guard lck_g(*motor_controller_write_mtx_); - - if (!enable) { - motors_controller_->TurnOnEStop(); - } else { - motors_controller_->TurnOffEStop(); - } - } - - e_stop_->TriggerEStop(); - - roboteq_error_filter_->SetClearErrorsFlag(); - roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); - } catch (const std::runtime_error & e) { - RCLCPP_WARN_STREAM(logger_, "An exception occurred while enabling motors power: " << e.what()); - } -} - -void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) -{ - unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; - std::string message{"No error detected."}; - - const auto front_driver_data = motors_controller_->GetFrontData(); - if (front_driver_data.IsError()) { - level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; - message = "Error detected."; - - panther_utils::diagnostics::AddKeyValueIfTrue( - status, front_driver_data.GetErrorMap(), "Front driver error: "); - } - - const auto rear_driver_data = motors_controller_->GetRearData(); - if (rear_driver_data.IsError()) { - level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; - message = "Error detected."; - - panther_utils::diagnostics::AddKeyValueIfTrue( - status, rear_driver_data.GetErrorMap(), "Rear driver error: "); - } - - if (roboteq_error_filter_->IsError()) { - level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; - message = "Error detected."; - - panther_utils::diagnostics::AddKeyValueIfTrue( - status, roboteq_error_filter_->GetErrorMap(), "", " error"); - } - - status.summary(level, message); -} - -void PantherSystem::DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) -{ - unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; - std::string message{"Panther system status monitoring."}; - - const auto front_driver_state = motors_controller_->GetFrontData().GetDriverState(); - const auto rear_driver_state = motors_controller_->GetRearData().GetDriverState(); - - auto drivers_states_with_names = { - std::make_pair(std::string("Front"), front_driver_state), - std::make_pair(std::string("Rear"), rear_driver_state)}; - - for (const auto & [driver_name, driver_state] : drivers_states_with_names) { - status.add(driver_name + " driver voltage (V)", driver_state.GetVoltage()); - status.add(driver_name + " driver current (A)", driver_state.GetCurrent()); - status.add(driver_name + " driver temperature (\u00B0C)", driver_state.GetTemperature()); - status.add( - driver_name + " driver heatsink temperature (\u00B0C)", - driver_state.GetHeatsinkTemperature()); - } - - status.summary(level, message); -} - -} // namespace panther_hardware_interfaces - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - panther_hardware_interfaces::PantherSystem, hardware_interface::SystemInterface) diff --git a/panther_hardware_interfaces/test/config/slave_2.bin b/panther_hardware_interfaces/test/config/slave_2.bin deleted file mode 100644 index c042c65d..00000000 Binary files a/panther_hardware_interfaces/test/config/slave_2.bin and /dev/null differ diff --git a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp b/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp deleted file mode 100644 index 2e89d879..00000000 --- a/panther_hardware_interfaces/test/panther_system/gpio/test_gpio_controller.cpp +++ /dev/null @@ -1,174 +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 - -using GPIOInfo = panther_hardware_interfaces::GPIOInfo; -using GPIOPin = panther_hardware_interfaces::GPIOPin; - -const std::vector gpio_config_info_storage{ - GPIOInfo{GPIOPin::WATCHDOG, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::AUX_PW_EN, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::CHRG_DISABLE, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::DRIVER_EN, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::E_STOP_RESET, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::FAN_SW, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::GPOUT1, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::GPOUT2, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::GPIN1, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::GPIN2, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::SHDN_INIT, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::VDIG_OFF, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::VMOT_ON, gpiod::line::direction::OUTPUT}, - GPIOInfo{GPIOPin::CHRG_SENSE, gpiod::line::direction::INPUT}, - GPIOInfo{GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT}, -}; - -class GPIOControllerWrapper : public panther_hardware_interfaces::GPIOControllerPTH12X -{ -public: - void WatchdogEnable() { this->watchdog_->TurnOn(); } - void WatchdogDisable() { this->watchdog_->TurnOff(); } - bool IsWatchdogEnabled() { return this->watchdog_->IsWatchdogEnabled(); } -}; - -class TestGPIOController : public ::testing::Test -{ -public: - TestGPIOController(); - ~TestGPIOController() { gpio_controller_wrapper_.reset(); } - -protected: - float GetRobotVersion(); - - std::unique_ptr gpio_controller_wrapper_; - static constexpr int watchdog_edges_per_100ms_ = 10; -}; - -TestGPIOController::TestGPIOController() -{ - if (GetRobotVersion() < 1.2 - std::numeric_limits::epsilon()) { - throw std::runtime_error("Tests for this robot versions are not implemented"); - } - - gpio_controller_wrapper_ = std::make_unique(); - gpio_controller_wrapper_->Start(); -} - -float TestGPIOController::GetRobotVersion() -{ - const char * robot_version_env = std::getenv("PANTHER_ROBOT_VERSION"); - - if (!robot_version_env) { - throw std::runtime_error("Can't read 'PANTHER_ROBOT_VERSION' environment variable"); - } - - return std::stof(robot_version_env); -} - -TEST_F(TestGPIOController, TestMotorsInit) -{ - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::VMOT_ON)); - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::DRIVER_EN)); -} - -TEST_F(TestGPIOController, TestWatchdog) -{ - auto edge_cnt = std::make_shared(0); - - gpio_controller_wrapper_->RegisterGPIOEventCallback([this, edge_cnt](const auto & state) mutable { - if (state.pin == GPIOPin::WATCHDOG) { - (*edge_cnt)++; - } - }); - - gpio_controller_wrapper_->WatchdogEnable(); - ASSERT_TRUE(gpio_controller_wrapper_->IsWatchdogEnabled()); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - gpio_controller_wrapper_->WatchdogDisable(); - - ASSERT_FALSE(gpio_controller_wrapper_->IsWatchdogEnabled()); - EXPECT_EQ(*edge_cnt, watchdog_edges_per_100ms_ + 1); - - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - EXPECT_EQ(*edge_cnt, watchdog_edges_per_100ms_ + 1); -} - -TEST_F(TestGPIOController, TestPinsAvailability) -{ - for (const auto & info : gpio_config_info_storage) { - EXPECT_TRUE(gpio_controller_wrapper_->IsPinAvailable(info.pin)); - } -} - -TEST_F(TestGPIOController, TestFanEnbale) -{ - gpio_controller_wrapper_->FanEnable(true); - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::FAN_SW)); - - gpio_controller_wrapper_->FanEnable(false); - EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::FAN_SW)); -} - -TEST_F(TestGPIOController, TestAUXPowerEnbale) -{ - gpio_controller_wrapper_->AUXPowerEnable(true); - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::AUX_PW_EN)); - - gpio_controller_wrapper_->AUXPowerEnable(false); - EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::AUX_PW_EN)); -} - -TEST_F(TestGPIOController, TestChargerEnable) -{ - gpio_controller_wrapper_->ChargerEnable(true); - EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::CHRG_DISABLE)); - - gpio_controller_wrapper_->ChargerEnable(false); - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::CHRG_DISABLE)); -} - -TEST_F(TestGPIOController, TestLEDControlEnable) -{ - gpio_controller_wrapper_->LEDControlEnable(true); - EXPECT_TRUE(gpio_controller_wrapper_->IsPinActive(GPIOPin::LED_SBC_SEL)); - - gpio_controller_wrapper_->LEDControlEnable(false); - EXPECT_FALSE(gpio_controller_wrapper_->IsPinActive(GPIOPin::LED_SBC_SEL)); -} - -TEST_F(TestGPIOController, TestQueryControlInterfaceIOStates) -{ - std::unordered_map io_states = - gpio_controller_wrapper_->QueryControlInterfaceIOStates(); - - ASSERT_EQ(io_states.size(), 7); - - for (const auto & [pin, expected_state] : io_states) { - bool actual_state = gpio_controller_wrapper_->IsPinActive(pin); - EXPECT_EQ(expected_state, actual_state); - } -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp deleted file mode 100644 index dc1c9a5f..00000000 --- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_canopen_controller.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2023 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 - -#include "utils/roboteqs_mock.hpp" -#include "utils/test_constants.hpp" - -class TestCANopenController : public ::testing::Test -{ -public: - TestCANopenController() - { - canopen_controller_ = std::make_unique( - panther_hardware_interfaces_test::kCANopenSettings); - - roboteqs_mock_ = std::make_unique(); - roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50)); - } - - ~TestCANopenController() - { - roboteqs_mock_->Stop(); - roboteqs_mock_.reset(); - } - -protected: - std::unique_ptr roboteqs_mock_; - std::unique_ptr canopen_controller_; -}; - -TEST_F(TestCANopenController, InitializeAndDeinitialize) -{ - ASSERT_NO_THROW(canopen_controller_->Initialize()); - ASSERT_NO_THROW(canopen_controller_->Deinitialize()); - - // Check if deinitialization worked correctly - initialize once again - ASSERT_NO_THROW(canopen_controller_->Initialize()); - ASSERT_NO_THROW(canopen_controller_->Deinitialize()); -} - -TEST_F(TestCANopenController, InitializeErrorDeviceType) -{ - roboteqs_mock_->GetFrontDriver()->SetOnReadWait(0x1000, 0, 100000); - ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error); - ASSERT_NO_THROW(canopen_controller_->Deinitialize()); - - roboteqs_mock_->GetFrontDriver()->SetOnReadWait(0x1000, 0, 0); - ASSERT_NO_THROW(canopen_controller_->Initialize()); - ASSERT_NO_THROW(canopen_controller_->Deinitialize()); -} - -TEST_F(TestCANopenController, InitializeErrorVendorId) -{ - roboteqs_mock_->GetRearDriver()->SetOnReadWait(0x1018, 1, 100000); - ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error); - ASSERT_NO_THROW(canopen_controller_->Deinitialize()); - - roboteqs_mock_->GetRearDriver()->SetOnReadWait(0x1018, 1, 0); - ASSERT_NO_THROW(canopen_controller_->Initialize()); - ASSERT_NO_THROW(canopen_controller_->Deinitialize()); -} - -TEST(TestCANopenControllerOthers, BootTimeout) -{ - std::unique_ptr canopen_controller_; - - canopen_controller_ = std::make_unique( - panther_hardware_interfaces_test::kCANopenSettings); - - // No roboteq mock, so it won't be possible to boot - here is checked if after some time it will - // finish with exception - ASSERT_THROW(canopen_controller_->Initialize(), std::runtime_error); - - canopen_controller_->Deinitialize(); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_motors_controller.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_motors_controller.cpp deleted file mode 100644 index 789415a8..00000000 --- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_motors_controller.cpp +++ /dev/null @@ -1,501 +0,0 @@ -// Copyright 2023 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 - -#include -#include -#include - -#include "utils/roboteqs_mock.hpp" -#include "utils/test_constants.hpp" - -class TestMotorsControllerInitialization : public ::testing::Test -{ -public: - TestMotorsControllerInitialization() - { - motors_controller_ = std::make_unique( - panther_hardware_interfaces_test::kCANopenSettings, - panther_hardware_interfaces_test::kDrivetrainSettings); - - roboteqs_mock_ = std::make_shared(); - roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50)); - } - - ~TestMotorsControllerInitialization() - { - roboteqs_mock_->Stop(); - roboteqs_mock_.reset(); - } - - std::shared_ptr roboteqs_mock_; - std::unique_ptr motors_controller_; -}; - -// These tests are related to canopen_controller tests, where boot should be already tested - -TEST_F(TestMotorsControllerInitialization, Initialize) -{ - ASSERT_NO_THROW(motors_controller_->Initialize()); - ASSERT_NO_THROW(motors_controller_->Deinitialize()); - - // Check if deinitialization worked correctly - initialize once again - ASSERT_NO_THROW(motors_controller_->Initialize()); - ASSERT_NO_THROW(motors_controller_->Deinitialize()); -} - -TEST_F(TestMotorsControllerInitialization, ErrorDeviceType) -{ - roboteqs_mock_->GetFrontDriver()->SetOnReadWait(0x1000, 0, 100000); - ASSERT_THROW(motors_controller_->Initialize(), std::runtime_error); - ASSERT_NO_THROW(motors_controller_->Deinitialize()); - - roboteqs_mock_->GetFrontDriver()->SetOnReadWait(0x1000, 0, 0); - ASSERT_NO_THROW(motors_controller_->Initialize()); - ASSERT_NO_THROW(motors_controller_->Deinitialize()); -} - -TEST_F(TestMotorsControllerInitialization, ErrorVendorId) -{ - roboteqs_mock_->GetRearDriver()->SetOnReadWait(0x1018, 1, 100000); - ASSERT_THROW(motors_controller_->Initialize(), std::runtime_error); - ASSERT_NO_THROW(motors_controller_->Deinitialize()); - - roboteqs_mock_->GetRearDriver()->SetOnReadWait(0x1018, 1, 0); - ASSERT_NO_THROW(motors_controller_->Initialize()); - ASSERT_NO_THROW(motors_controller_->Deinitialize()); -} - -TEST_F(TestMotorsControllerInitialization, Activate) -{ - using panther_hardware_interfaces_test::DriverChannel; - - ASSERT_NO_THROW(motors_controller_->Initialize()); - - roboteqs_mock_->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 234); - roboteqs_mock_->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 32); - roboteqs_mock_->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 54); - roboteqs_mock_->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 12); - - roboteqs_mock_->GetFrontDriver()->SetResetRoboteqScript(65); - roboteqs_mock_->GetRearDriver()->SetResetRoboteqScript(23); - - ASSERT_NO_THROW(motors_controller_->Activate()); - - ASSERT_EQ(roboteqs_mock_->GetFrontDriver()->GetResetRoboteqScript(), 2); - ASSERT_EQ(roboteqs_mock_->GetRearDriver()->GetResetRoboteqScript(), 2); - - ASSERT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - ASSERT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - - motors_controller_->Deinitialize(); -} - -TEST_F(TestMotorsControllerInitialization, ActivateSDOTimeoutReset) -{ - ASSERT_NO_THROW(motors_controller_->Initialize()); - roboteqs_mock_->GetFrontDriver()->SetOnWriteWait(0x2018, 0, 100000); - ASSERT_THROW(motors_controller_->Activate(), std::runtime_error); - ASSERT_NO_THROW(motors_controller_->Deinitialize()); -} - -class TestMotorsController : public TestMotorsControllerInitialization -{ -public: - TestMotorsController() - { - motors_controller_->Initialize(); - motors_controller_->Activate(); - } - - ~TestMotorsController() { motors_controller_->Deinitialize(); } -}; - -TEST_F(TestMotorsController, UpdateMotorsState) -{ - using panther_hardware_interfaces_test::DriverChannel; - - using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; - using panther_hardware_interfaces_test::kRbtqPosFbToRad; - using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec; - - const std::int32_t fl_pos = 101; - const std::int32_t fl_vel = 102; - const std::int32_t fl_current = 103; - const std::int32_t fr_pos = 201; - const std::int32_t fr_vel = 202; - const std::int32_t fr_current = 203; - const std::int32_t rl_pos = 301; - const std::int32_t rl_vel = 302; - const std::int32_t rl_current = 303; - const std::int32_t rr_pos = 401; - const std::int32_t rr_vel = 402; - const std::int32_t rr_current = 403; - - roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL2, fl_pos); - roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL1, fr_pos); - roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL2, rl_pos); - roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL1, rr_pos); - - roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL2, fl_vel); - roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL1, fr_vel); - roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL2, rl_vel); - roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL1, rr_vel); - - roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL2, fl_current); - roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL1, fr_current); - roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL2, rl_current); - roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL1, rr_current); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - motors_controller_->UpdateMotorsState(); - - const auto & fl = motors_controller_->GetFrontData().GetLeftMotorState(); - const auto & fr = motors_controller_->GetFrontData().GetRightMotorState(); - const auto & rl = motors_controller_->GetRearData().GetLeftMotorState(); - const auto & rr = motors_controller_->GetRearData().GetRightMotorState(); - - EXPECT_FLOAT_EQ(fl.GetPosition(), fl_pos * kRbtqPosFbToRad); - EXPECT_FLOAT_EQ(fl.GetVelocity(), fl_vel * kRbtqVelFbToRadPerSec); - EXPECT_FLOAT_EQ(fl.GetTorque(), fl_current * kRbtqCurrentFbToNewtonMeters); - - EXPECT_FLOAT_EQ(fr.GetPosition(), fr_pos * kRbtqPosFbToRad); - EXPECT_FLOAT_EQ(fr.GetVelocity(), fr_vel * kRbtqVelFbToRadPerSec); - EXPECT_FLOAT_EQ(fr.GetTorque(), fr_current * kRbtqCurrentFbToNewtonMeters); - - EXPECT_FLOAT_EQ(rl.GetPosition(), rl_pos * kRbtqPosFbToRad); - EXPECT_FLOAT_EQ(rl.GetVelocity(), rl_vel * kRbtqVelFbToRadPerSec); - EXPECT_FLOAT_EQ(rl.GetTorque(), rl_current * kRbtqCurrentFbToNewtonMeters); - - EXPECT_FLOAT_EQ(rr.GetPosition(), rr_pos * kRbtqPosFbToRad); - EXPECT_FLOAT_EQ(rr.GetVelocity(), rr_vel * kRbtqVelFbToRadPerSec); - EXPECT_FLOAT_EQ(rr.GetTorque(), rr_current * kRbtqCurrentFbToNewtonMeters); -} - -TEST_F(TestMotorsController, UpdateMotorsStateTimestamps) -{ - motors_controller_->UpdateMotorsState(); - - std::this_thread::sleep_for( - panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms + - std::chrono::milliseconds(10)); - - motors_controller_->UpdateMotorsState(); - - EXPECT_FALSE(motors_controller_->GetFrontData().IsMotorStatesDataTimedOut()); - EXPECT_FALSE(motors_controller_->GetRearData().IsMotorStatesDataTimedOut()); -} - -TEST(TestMotorsControllerOthers, UpdateMotorsStateTimeout) -{ - std::shared_ptr roboteqs_mock_; - std::unique_ptr motors_controller_; - - motors_controller_ = std::make_unique( - panther_hardware_interfaces_test::kCANopenSettings, - panther_hardware_interfaces_test::kDrivetrainSettings); - - roboteqs_mock_ = std::make_shared(); - - roboteqs_mock_->Start(std::chrono::milliseconds(200), std::chrono::milliseconds(50)); - - motors_controller_->Initialize(); - motors_controller_->Activate(); - - motors_controller_->UpdateMotorsState(); - - std::this_thread::sleep_for( - panther_hardware_interfaces_test::kCANopenSettings.pdo_motor_states_timeout_ms + - std::chrono::milliseconds(10)); - - motors_controller_->UpdateMotorsState(); - - EXPECT_TRUE(motors_controller_->GetFrontData().IsMotorStatesDataTimedOut()); - EXPECT_TRUE(motors_controller_->GetRearData().IsMotorStatesDataTimedOut()); - EXPECT_TRUE(motors_controller_->GetFrontData().IsError()); - EXPECT_TRUE(motors_controller_->GetRearData().IsError()); - - motors_controller_->Deinitialize(); - - roboteqs_mock_->Stop(); - roboteqs_mock_.reset(); -} - -// Similar to test_roboteq_driver, can_error in update_system_feedback isn't tested, because it -// reacts to lower-level CAN errors (CRC), which are hard to simulate, but it would be nice to add -// it - -TEST_F(TestMotorsController, UpdateDriverState) -{ - using panther_hardware_interfaces_test::DriverChannel; - using panther_hardware_interfaces_test::DriverFaultFlags; - using panther_hardware_interfaces_test::DriverRuntimeErrors; - using panther_hardware_interfaces_test::DriverScriptFlags; - - const std::int16_t f_temp = 30; - const std::int16_t r_temp = 32; - const std::int16_t f_heatsink_temp = 31; - const std::int16_t r_heatsink_temp = 33; - const std::uint16_t f_volt = 400; - const std::uint16_t r_volt = 430; - const std::int16_t f_battery_current_1 = 10; - const std::int16_t r_battery_current_1 = 30; - const std::int16_t f_battery_current_2 = 30; - const std::int16_t r_battery_current_2 = 40; - - roboteqs_mock_->GetFrontDriver()->SetTemperature(f_temp); - roboteqs_mock_->GetRearDriver()->SetTemperature(r_temp); - roboteqs_mock_->GetFrontDriver()->SetHeatsinkTemperature(f_heatsink_temp); - roboteqs_mock_->GetRearDriver()->SetHeatsinkTemperature(r_heatsink_temp); - roboteqs_mock_->GetFrontDriver()->SetVoltage(f_volt); - roboteqs_mock_->GetRearDriver()->SetVoltage(r_volt); - roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent1(f_battery_current_1); - roboteqs_mock_->GetRearDriver()->SetBatteryCurrent1(r_battery_current_1); - roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent2(f_battery_current_2); - roboteqs_mock_->GetRearDriver()->SetBatteryCurrent2(r_battery_current_2); - - roboteqs_mock_->GetFrontDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT); - roboteqs_mock_->GetFrontDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED); - roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError( - DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR); - roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError( - DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE); - - roboteqs_mock_->GetRearDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERVOLTAGE); - roboteqs_mock_->GetRearDriver()->SetDriverScriptFlag(DriverScriptFlags::AMP_LIMITER); - roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError( - DriverChannel::CHANNEL1, DriverRuntimeErrors::FORWARD_LIMIT_TRIGGERED); - roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError( - DriverChannel::CHANNEL2, DriverRuntimeErrors::REVERSE_LIMIT_TRIGGERED); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - motors_controller_->UpdateDriversState(); - - const auto & front = motors_controller_->GetFrontData(); - const auto & rear = motors_controller_->GetRearData(); - const auto & front_driver_state = motors_controller_->GetFrontData().GetDriverState(); - const auto & rear_driver_state = motors_controller_->GetRearData().GetDriverState(); - - EXPECT_EQ(static_cast(front_driver_state.GetTemperature()), f_temp); - EXPECT_EQ( - static_cast(front_driver_state.GetHeatsinkTemperature()), f_heatsink_temp); - EXPECT_EQ(static_cast(front_driver_state.GetVoltage() * 10.0), f_volt); - EXPECT_EQ( - static_cast(front_driver_state.GetCurrent() * 10.0), - f_battery_current_1 + f_battery_current_2); - - EXPECT_EQ(static_cast(rear_driver_state.GetTemperature()), r_temp); - EXPECT_EQ(static_cast(rear_driver_state.GetHeatsinkTemperature()), r_heatsink_temp); - EXPECT_EQ(static_cast(rear_driver_state.GetVoltage() * 10.0), r_volt); - EXPECT_EQ( - static_cast(rear_driver_state.GetCurrent() * 10.0), - r_battery_current_1 + r_battery_current_2); - - EXPECT_TRUE(front.GetFaultFlag().GetMessage().overheat); - EXPECT_TRUE(front.GetScriptFlag().GetMessage().encoder_disconnected); - EXPECT_TRUE(front.GetRightRuntimeError().GetMessage().loop_error); - EXPECT_TRUE(front.GetLeftRuntimeError().GetMessage().safety_stop_active); - - EXPECT_TRUE(rear.GetFaultFlag().GetMessage().overvoltage); - EXPECT_TRUE(rear.GetScriptFlag().GetMessage().amp_limiter); - EXPECT_TRUE(rear.GetRightRuntimeError().GetMessage().forward_limit_triggered); - EXPECT_TRUE(rear.GetLeftRuntimeError().GetMessage().reverse_limit_triggered); -} - -TEST_F(TestMotorsController, UpdateDriverStateTimestamps) -{ - motors_controller_->UpdateDriversState(); - - std::this_thread::sleep_for( - panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms + - std::chrono::milliseconds(10)); - - motors_controller_->UpdateDriversState(); - - EXPECT_FALSE(motors_controller_->GetFrontData().IsDriverStateDataTimedOut()); - EXPECT_FALSE(motors_controller_->GetRearData().IsDriverStateDataTimedOut()); -} - -TEST(TestMotorsControllerOthers, UpdateDriverStateTimeout) -{ - std::shared_ptr roboteqs_mock_; - std::unique_ptr motors_controller_; - - motors_controller_ = std::make_unique( - panther_hardware_interfaces_test::kCANopenSettings, - panther_hardware_interfaces_test::kDrivetrainSettings); - - roboteqs_mock_ = std::make_shared(); - - roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(200)); - - motors_controller_->Initialize(); - motors_controller_->Activate(); - - motors_controller_->UpdateDriversState(); - - std::this_thread::sleep_for( - panther_hardware_interfaces_test::kCANopenSettings.pdo_driver_state_timeout_ms + - std::chrono::milliseconds(10)); - - motors_controller_->UpdateDriversState(); - - EXPECT_TRUE(motors_controller_->GetFrontData().IsDriverStateDataTimedOut()); - EXPECT_TRUE(motors_controller_->GetRearData().IsDriverStateDataTimedOut()); - EXPECT_TRUE(motors_controller_->GetFrontData().IsError()); - EXPECT_TRUE(motors_controller_->GetRearData().IsError()); - - motors_controller_->Deinitialize(); - - roboteqs_mock_->Stop(); - roboteqs_mock_.reset(); -} - -TEST_F(TestMotorsController, WriteSpeed) -{ - using panther_hardware_interfaces_test::DriverChannel; - using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd; - - const float fl_v = 0.1; - const float fr_v = 0.2; - const float rl_v = 0.3; - const float rr_v = 0.4; - - ASSERT_NO_THROW(motors_controller_->SendSpeedCommands(fl_v, fr_v, rl_v, rr_v)); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - EXPECT_EQ( - roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(fl_v * kRadPerSecToRbtqCmd)); - EXPECT_EQ( - roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(fr_v * kRadPerSecToRbtqCmd)); - EXPECT_EQ( - roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(rl_v * kRadPerSecToRbtqCmd)); - EXPECT_EQ( - roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(rr_v * kRadPerSecToRbtqCmd)); -} - -// Similar to test_roboteq_driver, can_error in write speed isn't tested, because it reacts to lower -// level CAN errors (CRC), which are hard to simulate, but it would be nice to add it - -TEST_F(TestMotorsController, TurnOnEStop) -{ - roboteqs_mock_->GetFrontDriver()->SetTurnOnEStop(65); - roboteqs_mock_->GetRearDriver()->SetTurnOnEStop(23); - - ASSERT_NO_THROW(motors_controller_->TurnOnEStop()); - - EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnEStop(), 1); - EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnEStop(), 1); -} - -TEST_F(TestMotorsController, TurnOffEStop) -{ - roboteqs_mock_->GetFrontDriver()->SetTurnOffEStop(65); - roboteqs_mock_->GetRearDriver()->SetTurnOffEStop(23); - - ASSERT_NO_THROW(motors_controller_->TurnOffEStop()); - - EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOffEStop(), 1); - EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOffEStop(), 1); -} - -TEST_F(TestMotorsController, TurnOnEStopTimeout) -{ - roboteqs_mock_->GetFrontDriver()->SetOnWriteWait(0x200C, 0, 100000); - ASSERT_THROW(motors_controller_->TurnOnEStop(), std::runtime_error); -} - -TEST_F(TestMotorsController, TurnOffEStopTimeout) -{ - roboteqs_mock_->GetFrontDriver()->SetOnWriteWait(0x200D, 0, 100000); - ASSERT_THROW(motors_controller_->TurnOffEStop(), std::runtime_error); -} - -TEST_F(TestMotorsController, SafetyStop) -{ - roboteqs_mock_->GetFrontDriver()->SetTurnOnSafetyStop(65); - roboteqs_mock_->GetRearDriver()->SetTurnOnSafetyStop(23); - - bool front_driver_channel1_safety_stop = false; - bool rear_driver_channel1_safety_stop = false; - - std::atomic_bool finish_test = false; - - // Check if first channel was set in the meantime - not sure how robust this test will be - as - // safety stops for channel 1 and 2 are set just after one another, it is necessary to check value - // of the current channel set frequently (and performance can vary on different machines) - auto channel1_test_thread = std::thread([roboteqs_mock = roboteqs_mock_, &finish_test, - &front_driver_channel1_safety_stop, - &rear_driver_channel1_safety_stop]() { - while (true) { - if ( - front_driver_channel1_safety_stop == false && - roboteqs_mock->GetFrontDriver()->GetTurnOnSafetyStop() == 1) { - front_driver_channel1_safety_stop = true; - } - - if ( - rear_driver_channel1_safety_stop == false && - roboteqs_mock->GetRearDriver()->GetTurnOnSafetyStop() == 1) { - rear_driver_channel1_safety_stop = true; - } - - if (finish_test || (front_driver_channel1_safety_stop && rear_driver_channel1_safety_stop)) { - break; - } - - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - }); - - ASSERT_NO_THROW(motors_controller_->TurnOnSafetyStop()); - - finish_test = true; - channel1_test_thread.join(); - - ASSERT_TRUE(front_driver_channel1_safety_stop); - ASSERT_TRUE(rear_driver_channel1_safety_stop); - - ASSERT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnSafetyStop(), 2); - ASSERT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnSafetyStop(), 2); -} - -TEST_F(TestMotorsController, SafetyStopTimeout) -{ - roboteqs_mock_->GetFrontDriver()->SetOnWriteWait(0x202C, 0, 100000); - ASSERT_THROW(motors_controller_->TurnOnSafetyStop(), std::runtime_error); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp b/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp deleted file mode 100644 index 7f83e88d..00000000 --- a/panther_hardware_interfaces/test/panther_system/motors_controller/test_roboteq_driver.cpp +++ /dev/null @@ -1,390 +0,0 @@ -// Copyright 2023 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 -#include - -#include "utils/roboteqs_mock.hpp" -#include "utils/test_constants.hpp" - -#include - -class TestRoboteqDriver : public ::testing::Test -{ -public: - TestRoboteqDriver() - { - canopen_controller_ = std::make_unique( - panther_hardware_interfaces_test::kCANopenSettings); - - roboteqs_mock_ = std::make_unique(); - roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50)); - canopen_controller_->Initialize(); - } - - ~TestRoboteqDriver() - { - canopen_controller_->Deinitialize(); - roboteqs_mock_->Stop(); - roboteqs_mock_.reset(); - } - -protected: - std::unique_ptr roboteqs_mock_; - std::unique_ptr canopen_controller_; -}; - -TEST_F(TestRoboteqDriver, ReadRoboteqMotorStates) -{ - using panther_hardware_interfaces_test::DriverChannel; - - const std::int32_t fl_pos = 101; - const std::int32_t fl_vel = 102; - const std::int32_t fl_current = 103; - const std::int32_t fr_pos = 201; - const std::int32_t fr_vel = 202; - const std::int32_t fr_current = 203; - const std::int32_t rl_pos = 301; - const std::int32_t rl_vel = 302; - const std::int32_t rl_current = 303; - const std::int32_t rr_pos = 401; - const std::int32_t rr_vel = 402; - const std::int32_t rr_current = 403; - - roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL2, fl_pos); - roboteqs_mock_->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL1, fr_pos); - roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL2, rl_pos); - roboteqs_mock_->GetRearDriver()->SetPosition(DriverChannel::CHANNEL1, rr_pos); - - roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL2, fl_vel); - roboteqs_mock_->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL1, fr_vel); - roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL2, rl_vel); - roboteqs_mock_->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL1, rr_vel); - - roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL2, fl_current); - roboteqs_mock_->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL1, fr_current); - roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL2, rl_current); - roboteqs_mock_->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL1, rr_current); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - panther_hardware_interfaces::RoboteqMotorsStates f_fb = - canopen_controller_->GetFrontDriver()->ReadRoboteqMotorsStates(); - panther_hardware_interfaces::RoboteqMotorsStates r_fb = - canopen_controller_->GetRearDriver()->ReadRoboteqMotorsStates(); - - EXPECT_EQ(f_fb.motor_2.pos, fl_pos); - EXPECT_EQ(f_fb.motor_2.vel, fl_vel); - EXPECT_EQ(f_fb.motor_2.current, fl_current); - - EXPECT_EQ(f_fb.motor_1.pos, fr_pos); - EXPECT_EQ(f_fb.motor_1.vel, fr_vel); - EXPECT_EQ(f_fb.motor_1.current, fr_current); - - EXPECT_EQ(r_fb.motor_2.pos, rl_pos); - EXPECT_EQ(r_fb.motor_2.vel, rl_vel); - EXPECT_EQ(r_fb.motor_2.current, rl_current); - - EXPECT_EQ(r_fb.motor_1.pos, rr_pos); - EXPECT_EQ(r_fb.motor_1.vel, rr_vel); - EXPECT_EQ(r_fb.motor_1.current, rr_current); -} - -TEST_F(TestRoboteqDriver, ReadRoboteqMotorStatesTimestamps) -{ - std::this_thread::sleep_for(std::chrono::milliseconds(150)); - - panther_hardware_interfaces::RoboteqMotorsStates f_fb1 = - canopen_controller_->GetFrontDriver()->ReadRoboteqMotorsStates(); - panther_hardware_interfaces::RoboteqMotorsStates r_fb1 = - canopen_controller_->GetRearDriver()->ReadRoboteqMotorsStates(); - - // based on publishing frequency in the Roboteq mock (10) - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - panther_hardware_interfaces::RoboteqMotorsStates f_fb2 = - canopen_controller_->GetFrontDriver()->ReadRoboteqMotorsStates(); - panther_hardware_interfaces::RoboteqMotorsStates r_fb2 = - canopen_controller_->GetRearDriver()->ReadRoboteqMotorsStates(); - - // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked - // if consecutive messages will have timestamps 100ms + some threshold apart - EXPECT_LE( - lely::util::from_timespec(f_fb2.pos_timestamp) - lely::util::from_timespec(f_fb1.pos_timestamp), - std::chrono::milliseconds(15)); - EXPECT_LE( - lely::util::from_timespec(r_fb2.pos_timestamp) - lely::util::from_timespec(r_fb1.pos_timestamp), - std::chrono::milliseconds(15)); - - EXPECT_GE( - lely::util::from_timespec(f_fb2.pos_timestamp) - lely::util::from_timespec(f_fb1.pos_timestamp), - std::chrono::milliseconds(5)); - EXPECT_GE( - lely::util::from_timespec(r_fb2.pos_timestamp) - lely::util::from_timespec(r_fb1.pos_timestamp), - std::chrono::milliseconds(5)); - - EXPECT_LE( - lely::util::from_timespec(f_fb2.vel_current_timestamp) - - lely::util::from_timespec(f_fb1.vel_current_timestamp), - std::chrono::milliseconds(15)); - EXPECT_LE( - lely::util::from_timespec(r_fb2.vel_current_timestamp) - - lely::util::from_timespec(r_fb1.vel_current_timestamp), - std::chrono::milliseconds(15)); - - EXPECT_GE( - lely::util::from_timespec(f_fb2.vel_current_timestamp) - - lely::util::from_timespec(f_fb1.vel_current_timestamp), - std::chrono::milliseconds(5)); - EXPECT_GE( - lely::util::from_timespec(r_fb2.vel_current_timestamp) - - lely::util::from_timespec(r_fb1.vel_current_timestamp), - std::chrono::milliseconds(5)); -} - -TEST_F(TestRoboteqDriver, ReadRoboteqDriverState) -{ - using panther_hardware_interfaces_test::DriverChannel; - using panther_hardware_interfaces_test::DriverFaultFlags; - using panther_hardware_interfaces_test::DriverRuntimeErrors; - using panther_hardware_interfaces_test::DriverScriptFlags; - - const std::int16_t f_temp = 30; - const std::int16_t r_temp = 32; - const std::int16_t f_heatsink_temp = 31; - const std::int16_t r_heatsink_temp = 33; - const std::uint16_t f_volt = 400; - const std::uint16_t r_volt = 430; - const std::int16_t f_battery_current_1 = 10; - const std::int16_t r_battery_current_1 = 30; - const std::int16_t f_battery_current_2 = 30; - const std::int16_t r_battery_current_2 = 40; - - roboteqs_mock_->GetFrontDriver()->SetTemperature(f_temp); - roboteqs_mock_->GetRearDriver()->SetTemperature(r_temp); - roboteqs_mock_->GetFrontDriver()->SetHeatsinkTemperature(f_heatsink_temp); - roboteqs_mock_->GetRearDriver()->SetHeatsinkTemperature(r_heatsink_temp); - roboteqs_mock_->GetFrontDriver()->SetVoltage(f_volt); - roboteqs_mock_->GetRearDriver()->SetVoltage(r_volt); - roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent1(f_battery_current_1); - roboteqs_mock_->GetRearDriver()->SetBatteryCurrent1(r_battery_current_1); - roboteqs_mock_->GetFrontDriver()->SetBatteryCurrent2(f_battery_current_2); - roboteqs_mock_->GetRearDriver()->SetBatteryCurrent2(r_battery_current_2); - - roboteqs_mock_->GetFrontDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERHEAT); - roboteqs_mock_->GetFrontDriver()->SetDriverScriptFlag(DriverScriptFlags::ENCODER_DISCONNECTED); - roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError( - DriverChannel::CHANNEL1, DriverRuntimeErrors::LOOP_ERROR); - roboteqs_mock_->GetFrontDriver()->SetDriverRuntimeError( - DriverChannel::CHANNEL2, DriverRuntimeErrors::SAFETY_STOP_ACTIVE); - - roboteqs_mock_->GetRearDriver()->SetDriverFaultFlag(DriverFaultFlags::OVERVOLTAGE); - roboteqs_mock_->GetRearDriver()->SetDriverScriptFlag(DriverScriptFlags::AMP_LIMITER); - roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError( - DriverChannel::CHANNEL1, DriverRuntimeErrors::FORWARD_LIMIT_TRIGGERED); - roboteqs_mock_->GetRearDriver()->SetDriverRuntimeError( - DriverChannel::CHANNEL2, DriverRuntimeErrors::REVERSE_LIMIT_TRIGGERED); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - panther_hardware_interfaces::RoboteqDriverState f_fb = - canopen_controller_->GetFrontDriver()->ReadRoboteqDriverState(); - panther_hardware_interfaces::RoboteqDriverState r_fb = - canopen_controller_->GetRearDriver()->ReadRoboteqDriverState(); - - EXPECT_EQ(f_fb.mcu_temp, f_temp); - EXPECT_EQ(r_fb.mcu_temp, r_temp); - - EXPECT_EQ(f_fb.heatsink_temp, f_heatsink_temp); - EXPECT_EQ(r_fb.heatsink_temp, r_heatsink_temp); - - EXPECT_EQ(f_fb.battery_voltage, f_volt); - EXPECT_EQ(r_fb.battery_voltage, r_volt); - - EXPECT_EQ(f_fb.battery_current_1, f_battery_current_1); - EXPECT_EQ(r_fb.battery_current_1, r_battery_current_1); - - EXPECT_EQ(f_fb.battery_current_2, f_battery_current_2); - EXPECT_EQ(r_fb.battery_current_2, r_battery_current_2); - - EXPECT_EQ(f_fb.fault_flags, 0b00000001); - EXPECT_EQ(f_fb.script_flags, 0b00000010); - EXPECT_EQ(f_fb.runtime_stat_flag_motor_1, 0b00000100); - EXPECT_EQ(f_fb.runtime_stat_flag_motor_2, 0b00001000); - - EXPECT_EQ(r_fb.fault_flags, 0b00000010); - EXPECT_EQ(r_fb.script_flags, 0b00000100); - EXPECT_EQ(r_fb.runtime_stat_flag_motor_1, 0b00010000); - EXPECT_EQ(r_fb.runtime_stat_flag_motor_2, 0b00100000); -} - -TEST_F(TestRoboteqDriver, ReadRoboteqDriverStateTimestamp) -{ - std::this_thread::sleep_for(std::chrono::milliseconds(150)); - - panther_hardware_interfaces::RoboteqDriverState f_fb1 = - canopen_controller_->GetFrontDriver()->ReadRoboteqDriverState(); - panther_hardware_interfaces::RoboteqDriverState r_fb1 = - canopen_controller_->GetRearDriver()->ReadRoboteqDriverState(); - - // based on publishing frequency in the Roboteq mock (50) - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - - panther_hardware_interfaces::RoboteqDriverState f_fb2 = - canopen_controller_->GetFrontDriver()->ReadRoboteqDriverState(); - panther_hardware_interfaces::RoboteqDriverState r_fb2 = - canopen_controller_->GetRearDriver()->ReadRoboteqDriverState(); - - // feedback is published with a 100ms period, to check if timestamps are accurate, it is checked - // if consecutive messages will have timestamps 100ms + some threshold apart - EXPECT_LE( - lely::util::from_timespec(f_fb2.flags_current_timestamp) - - lely::util::from_timespec(f_fb1.flags_current_timestamp), - std::chrono::milliseconds(75)); - EXPECT_LE( - lely::util::from_timespec(r_fb2.flags_current_timestamp) - - lely::util::from_timespec(r_fb1.flags_current_timestamp), - std::chrono::milliseconds(75)); - - EXPECT_GE( - lely::util::from_timespec(f_fb2.flags_current_timestamp) - - lely::util::from_timespec(f_fb1.flags_current_timestamp), - std::chrono::milliseconds(25)); - EXPECT_GE( - lely::util::from_timespec(r_fb2.flags_current_timestamp) - - lely::util::from_timespec(r_fb1.flags_current_timestamp), - std::chrono::milliseconds(25)); - - EXPECT_LE( - lely::util::from_timespec(f_fb2.voltages_temps_timestamp) - - lely::util::from_timespec(f_fb1.voltages_temps_timestamp), - std::chrono::milliseconds(75)); - EXPECT_LE( - lely::util::from_timespec(r_fb2.voltages_temps_timestamp) - - lely::util::from_timespec(r_fb1.voltages_temps_timestamp), - std::chrono::milliseconds(75)); - - EXPECT_GE( - lely::util::from_timespec(f_fb2.voltages_temps_timestamp) - - lely::util::from_timespec(f_fb1.voltages_temps_timestamp), - std::chrono::milliseconds(25)); - EXPECT_GE( - lely::util::from_timespec(r_fb2.voltages_temps_timestamp) - - lely::util::from_timespec(r_fb1.voltages_temps_timestamp), - std::chrono::milliseconds(25)); -} - -TEST_F(TestRoboteqDriver, SendRoboteqCmd) -{ - using panther_hardware_interfaces_test::DriverChannel; - - const std::int32_t fl_v = 10; - const std::int32_t fr_v = 20; - const std::int32_t rl_v = 30; - const std::int32_t rr_v = 40; - - canopen_controller_->GetFrontDriver()->SendRoboteqCmd(fr_v, fl_v); - canopen_controller_->GetRearDriver()->SendRoboteqCmd(rr_v, rl_v); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), fl_v); - EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), fr_v); - EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), rl_v); - EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), rr_v); -} - -TEST_F(TestRoboteqDriver, ResetRoboteqScript) -{ - roboteqs_mock_->GetFrontDriver()->SetResetRoboteqScript(65); - roboteqs_mock_->GetRearDriver()->SetResetRoboteqScript(23); - - canopen_controller_->GetFrontDriver()->ResetRoboteqScript(); - canopen_controller_->GetRearDriver()->ResetRoboteqScript(); - - EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetResetRoboteqScript(), 2); - EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetResetRoboteqScript(), 2); -} - -TEST_F(TestRoboteqDriver, ReadRoboteqTurnOnEStop) -{ - roboteqs_mock_->GetFrontDriver()->SetTurnOnEStop(65); - roboteqs_mock_->GetRearDriver()->SetTurnOnEStop(23); - - canopen_controller_->GetFrontDriver()->TurnOnEStop(); - canopen_controller_->GetRearDriver()->TurnOnEStop(); - - EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnEStop(), 1); - EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnEStop(), 1); -} - -TEST_F(TestRoboteqDriver, TurnOffEStop) -{ - roboteqs_mock_->GetFrontDriver()->SetTurnOffEStop(65); - roboteqs_mock_->GetRearDriver()->SetTurnOffEStop(23); - - canopen_controller_->GetFrontDriver()->TurnOffEStop(); - canopen_controller_->GetRearDriver()->TurnOffEStop(); - - EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOffEStop(), 1); - EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOffEStop(), 1); -} - -TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel1) -{ - roboteqs_mock_->GetFrontDriver()->SetTurnOnSafetyStop(67); - roboteqs_mock_->GetRearDriver()->SetTurnOnSafetyStop(21); - - canopen_controller_->GetFrontDriver()->TurnOnSafetyStopChannel1(); - canopen_controller_->GetRearDriver()->TurnOnSafetyStopChannel1(); - - EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnSafetyStop(), 1); - EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnSafetyStop(), 1); -} - -TEST_F(TestRoboteqDriver, TurnOnSafetyStopChannel2) -{ - roboteqs_mock_->GetFrontDriver()->SetTurnOnSafetyStop(65); - roboteqs_mock_->GetRearDriver()->SetTurnOnSafetyStop(23); - - canopen_controller_->GetFrontDriver()->TurnOnSafetyStopChannel2(); - canopen_controller_->GetRearDriver()->TurnOnSafetyStopChannel2(); - - EXPECT_EQ(roboteqs_mock_->GetFrontDriver()->GetTurnOnSafetyStop(), 2); - EXPECT_EQ(roboteqs_mock_->GetRearDriver()->GetTurnOnSafetyStop(), 2); -} - -TEST_F(TestRoboteqDriver, WriteTimeout) -{ - roboteqs_mock_->GetFrontDriver()->SetOnWriteWait(0x202C, 0, 200000); - ASSERT_THROW( - canopen_controller_->GetFrontDriver()->TurnOnSafetyStopChannel1(), std::runtime_error); -} - -// OnCanError isn't tested, because it reacts to lower-level CAN errors (CRC), which are hard to -// simulate, but it would be nice to add it - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp deleted file mode 100644 index 6ea1cf17..00000000 --- a/panther_hardware_interfaces/test/panther_system/test_panther_system.cpp +++ /dev/null @@ -1,769 +0,0 @@ -// Copyright 2023 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 - -#include - -#include -#include - -#include - -#include - -#include - -#include "utils/panther_system_test_utils.hpp" -#include "utils/roboteqs_mock.hpp" - -class TestPantherSystem : public ::testing::Test -{ -public: - TestPantherSystem() { pth_test_.Start(pth_test_.GetDefaultPantherSystemUrdf()); } - ~TestPantherSystem() { pth_test_.Stop(); } - - void CheckInterfaces() - { - EXPECT_EQ(pth_test_.GetResourceManager()->system_components_size(), 1u); - ASSERT_EQ(pth_test_.GetResourceManager()->state_interface_keys().size(), 12u); - - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fl_wheel_joint/position")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fr_wheel_joint/position")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rl_wheel_joint/position")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rr_wheel_joint/position")); - - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fl_wheel_joint/velocity")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fr_wheel_joint/velocity")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rl_wheel_joint/velocity")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rr_wheel_joint/velocity")); - - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fl_wheel_joint/effort")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("fr_wheel_joint/effort")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rl_wheel_joint/effort")); - EXPECT_TRUE(pth_test_.GetResourceManager()->state_interface_exists("rr_wheel_joint/effort")); - - ASSERT_EQ(pth_test_.GetResourceManager()->command_interface_keys().size(), 4u); - - EXPECT_TRUE( - pth_test_.GetResourceManager()->command_interface_exists("fl_wheel_joint/velocity")); - EXPECT_TRUE( - pth_test_.GetResourceManager()->command_interface_exists("fr_wheel_joint/velocity")); - EXPECT_TRUE( - pth_test_.GetResourceManager()->command_interface_exists("rl_wheel_joint/velocity")); - EXPECT_TRUE( - pth_test_.GetResourceManager()->command_interface_exists("rr_wheel_joint/velocity")); - } - - void CheckInitialValues() - { - using hardware_interface::LoanedCommandInterface; - using hardware_interface::LoanedStateInterface; - - LoanedStateInterface fl_s_p = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/position"); - LoanedStateInterface fr_s_p = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/position"); - LoanedStateInterface rl_s_p = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/position"); - LoanedStateInterface rr_s_p = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/position"); - - LoanedStateInterface fl_s_v = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/velocity"); - LoanedStateInterface fr_s_v = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/velocity"); - LoanedStateInterface rl_s_v = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/velocity"); - LoanedStateInterface rr_s_v = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/velocity"); - - LoanedStateInterface fl_s_e = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/effort"); - LoanedStateInterface fr_s_e = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/effort"); - LoanedStateInterface rl_s_e = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/effort"); - LoanedStateInterface rr_s_e = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/effort"); - - LoanedCommandInterface fl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity"); - LoanedCommandInterface fr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity"); - LoanedCommandInterface rl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity"); - LoanedCommandInterface rr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity"); - - ASSERT_FLOAT_EQ(fl_s_p.get_value(), 0.0); - ASSERT_FLOAT_EQ(fr_s_p.get_value(), 0.0); - ASSERT_FLOAT_EQ(rl_s_p.get_value(), 0.0); - ASSERT_FLOAT_EQ(rr_s_p.get_value(), 0.0); - - ASSERT_FLOAT_EQ(fl_s_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(fr_s_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(rl_s_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(rr_s_v.get_value(), 0.0); - - ASSERT_FLOAT_EQ(fl_s_e.get_value(), 0.0); - ASSERT_FLOAT_EQ(fr_s_e.get_value(), 0.0); - ASSERT_FLOAT_EQ(rl_s_e.get_value(), 0.0); - ASSERT_FLOAT_EQ(rr_s_e.get_value(), 0.0); - - ASSERT_FLOAT_EQ(fl_c_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(fr_c_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(rl_c_v.get_value(), 0.0); - ASSERT_FLOAT_EQ(rr_c_v.get_value(), 0.0); - } - - // 100 Hz - const float period_ = 0.01; - - panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_; -}; - -// TRANSITIONS -TEST_F(TestPantherSystem, ConfigureActivateFinalizePantherSystem) -{ - using panther_hardware_interfaces_test::kPantherSystemName; - - // check if hardware is configured - auto status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); - - try { - pth_test_.ConfigurePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ConfigurePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - - try { - pth_test_.ActivatePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ActivatePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::ACTIVE); - - // Check interfaces - CheckInterfaces(); - - // Check initial values - CheckInitialValues(); - - try { - pth_test_.ShutdownPantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ShutdownPantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::FINALIZED); -} - -TEST_F(TestPantherSystem, ConfigureActivateDeactivateDeconfigurePantherSystem) -{ - using panther_hardware_interfaces_test::kPantherSystemName; - - auto status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); - - try { - pth_test_.ConfigurePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ConfigurePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - - try { - pth_test_.ActivatePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to ActivatePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::ACTIVE); - - // Check interfaces - CheckInterfaces(); - - // Check initial values - CheckInitialValues(); - - try { - pth_test_.DeactivatePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to DeactivatePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - - try { - pth_test_.UnconfigurePantherSystem(); - } catch (const std::exception & e) { - FAIL() << "Exception caught when trying to UnconfigurePantherSystem: " << e.what(); - return; - } - status_map = pth_test_.GetResourceManager()->get_components_status(); - ASSERT_EQ( - status_map[kPantherSystemName].state.label(), - hardware_interface::lifecycle_state_names::UNCONFIGURED); -} - -// WRITING -TEST_F(TestPantherSystem, WriteCommandsPantherSystem) -{ - using hardware_interface::LoanedCommandInterface; - - using panther_hardware_interfaces_test::DriverChannel; - - using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd; - - const float fl_v = 0.1; - const float fr_v = 0.2; - const float rl_v = 0.3; - const float rr_v = 0.4; - - pth_test_.ConfigureActivatePantherSystem(); - - LoanedCommandInterface fl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity"); - LoanedCommandInterface fr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity"); - LoanedCommandInterface rl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity"); - LoanedCommandInterface rr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity"); - - fl_c_v.set_value(fl_v); - fr_c_v.set_value(fr_v); - rl_c_v.set_value(rl_v); - rr_c_v.set_value(rr_v); - - ASSERT_FLOAT_EQ(fl_c_v.get_value(), fl_v); - ASSERT_FLOAT_EQ(fr_c_v.get_value(), fr_v); - ASSERT_FLOAT_EQ(rl_c_v.get_value(), rl_v); - ASSERT_FLOAT_EQ(rr_c_v.get_value(), rr_v); - - const auto TIME = rclcpp::Time(0); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - - pth_test_.GetResourceManager()->write(TIME, PERIOD); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(fl_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(fr_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(rl_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(rr_v * kRadPerSecToRbtqCmd)); - - pth_test_.ShutdownPantherSystem(); -} - -// READING -TEST_F(TestPantherSystem, ReadFeedbackPantherSystem) -{ - using hardware_interface::LoanedStateInterface; - - using panther_hardware_interfaces_test::DriverChannel; - - using panther_hardware_interfaces_test::kRbtqCurrentFbToNewtonMeters; - using panther_hardware_interfaces_test::kRbtqPosFbToRad; - using panther_hardware_interfaces_test::kRbtqVelFbToRadPerSec; - - const std::int32_t fl_val = 100; - const std::int32_t fr_val = 200; - const std::int32_t rl_val = 300; - const std::int32_t rr_val = 400; - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL2, fl_val); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetPosition(DriverChannel::CHANNEL1, fr_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetPosition(DriverChannel::CHANNEL2, rl_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetPosition(DriverChannel::CHANNEL1, rr_val); - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL2, fl_val); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetVelocity(DriverChannel::CHANNEL1, fr_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL2, rl_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetVelocity(DriverChannel::CHANNEL1, rr_val); - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL2, fl_val); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetCurrent(DriverChannel::CHANNEL1, fr_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL2, rl_val); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetCurrent(DriverChannel::CHANNEL1, rr_val); - - pth_test_.ConfigureActivatePantherSystem(); - - LoanedStateInterface fl_s_p = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/position"); - LoanedStateInterface fr_s_p = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/position"); - LoanedStateInterface rl_s_p = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/position"); - LoanedStateInterface rr_s_p = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/position"); - - LoanedStateInterface fl_s_v = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/velocity"); - LoanedStateInterface fr_s_v = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/velocity"); - LoanedStateInterface rl_s_v = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/velocity"); - LoanedStateInterface rr_s_v = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/velocity"); - - LoanedStateInterface fl_s_e = - pth_test_.GetResourceManager()->claim_state_interface("fl_wheel_joint/effort"); - LoanedStateInterface fr_s_e = - pth_test_.GetResourceManager()->claim_state_interface("fr_wheel_joint/effort"); - LoanedStateInterface rl_s_e = - pth_test_.GetResourceManager()->claim_state_interface("rl_wheel_joint/effort"); - LoanedStateInterface rr_s_e = - pth_test_.GetResourceManager()->claim_state_interface("rr_wheel_joint/effort"); - - const auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - try { - pth_test_.GetResourceManager()->read(TIME, PERIOD); - } catch (const std::exception & e) { - FAIL() << "Exception: " << e.what(); - return; - } - - ASSERT_FLOAT_EQ(fl_s_p.get_value(), fl_val * kRbtqPosFbToRad); - ASSERT_FLOAT_EQ(fr_s_p.get_value(), fr_val * kRbtqPosFbToRad); - ASSERT_FLOAT_EQ(rl_s_p.get_value(), rl_val * kRbtqPosFbToRad); - ASSERT_FLOAT_EQ(rr_s_p.get_value(), rr_val * kRbtqPosFbToRad); - - ASSERT_FLOAT_EQ(fl_s_v.get_value(), fl_val * kRbtqVelFbToRadPerSec); - ASSERT_FLOAT_EQ(fr_s_v.get_value(), fr_val * kRbtqVelFbToRadPerSec); - ASSERT_FLOAT_EQ(rl_s_v.get_value(), rl_val * kRbtqVelFbToRadPerSec); - ASSERT_FLOAT_EQ(rr_s_v.get_value(), rr_val * kRbtqVelFbToRadPerSec); - - ASSERT_FLOAT_EQ(fl_s_e.get_value(), fl_val * kRbtqCurrentFbToNewtonMeters); - ASSERT_FLOAT_EQ(fr_s_e.get_value(), fr_val * kRbtqCurrentFbToNewtonMeters); - ASSERT_FLOAT_EQ(rl_s_e.get_value(), rl_val * kRbtqCurrentFbToNewtonMeters); - ASSERT_FLOAT_EQ(rr_s_e.get_value(), rr_val * kRbtqCurrentFbToNewtonMeters); - - pth_test_.ShutdownPantherSystem(); -} - -TEST_F(TestPantherSystem, ReadOtherRoboteqParamsPantherSystem) -{ - using hardware_interface::LoanedStateInterface; - - const std::int16_t f_temp = 30; - const std::int16_t r_temp = 32; - const std::int16_t f_heatsink_temp = 31; - const std::int16_t r_heatsink_temp = 33; - const std::uint16_t f_volt = 400; - const std::uint16_t r_volt = 430; - const std::int16_t f_battery_current_1 = 10; - const std::int16_t r_battery_current_1 = 30; - const std::int16_t f_battery_current_2 = 30; - const std::int16_t r_battery_current_2 = 40; - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetTemperature(f_temp); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetTemperature(r_temp); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetHeatsinkTemperature(f_heatsink_temp); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetHeatsinkTemperature(r_heatsink_temp); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetVoltage(f_volt); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetVoltage(r_volt); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetBatteryCurrent1(f_battery_current_1); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetBatteryCurrent1(r_battery_current_1); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetBatteryCurrent2(f_battery_current_2); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetBatteryCurrent2(r_battery_current_2); - - rclcpp::Node::SharedPtr node = std::make_shared("hardware_interface_test_node"); - - pth_test_.ConfigureActivatePantherSystem(); - - panther_msgs::msg::DriverState::SharedPtr state_msg; - unsigned state_msg_count = 0; - auto sub = node->create_subscription( - panther_hardware_interfaces_test::kMotorControllersStateTopic, rclcpp::SensorDataQoS(), - [&](const panther_msgs::msg::DriverState::SharedPtr msg) { - state_msg = msg; - ++state_msg_count; - }); - - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto simulated_time = node->get_clock()->now(); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - - try { - pth_test_.GetResourceManager()->read(simulated_time, PERIOD); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); - } catch (const std::exception & e) { - FAIL() << "Exception: " << e.what(); - return; - } - - ASSERT_TRUE(state_msg); - - ASSERT_EQ(static_cast(state_msg->front.temperature), f_temp); - ASSERT_EQ(static_cast(state_msg->rear.temperature), r_temp); - - ASSERT_EQ(static_cast(state_msg->front.heatsink_temperature), f_heatsink_temp); - ASSERT_EQ(static_cast(state_msg->rear.heatsink_temperature), r_heatsink_temp); - - ASSERT_EQ(static_cast(state_msg->front.voltage * 10.0), f_volt); - ASSERT_EQ(static_cast(state_msg->rear.voltage * 10.0), r_volt); - - ASSERT_EQ( - static_cast(state_msg->front.current * 10.0), - (f_battery_current_1 + f_battery_current_2)); - ASSERT_EQ( - static_cast(state_msg->rear.current * 10.0), - (r_battery_current_1 + r_battery_current_2)); - - pth_test_.ShutdownPantherSystem(); -} - -// ENCODER DISCONNECTED -TEST_F(TestPantherSystem, EncoderDisconnectedPantherSystem) -{ - using hardware_interface::LoanedCommandInterface; - using panther_hardware_interfaces_test::DriverChannel; - using panther_hardware_interfaces_test::DriverScriptFlags; - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetDriverScriptFlag( - DriverScriptFlags::ENCODER_DISCONNECTED); - - rclcpp::Node::SharedPtr node = std::make_shared("hardware_interface_test_node"); - - pth_test_.ConfigureActivatePantherSystem(); - - panther_msgs::msg::DriverState::SharedPtr state_msg; - auto sub = node->create_subscription( - panther_hardware_interfaces_test::kMotorControllersStateTopic, rclcpp::SensorDataQoS(), - [&](const panther_msgs::msg::DriverState::SharedPtr msg) { state_msg = msg; }); - - std::this_thread::sleep_for(std::chrono::seconds(2)); - - const auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - - pth_test_.GetResourceManager()->read(TIME, PERIOD); - - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); - ASSERT_TRUE(state_msg->front.script_flag.encoder_disconnected); - - // writing should be blocked - error - - LoanedCommandInterface fl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity"); - LoanedCommandInterface fr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity"); - LoanedCommandInterface rl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity"); - LoanedCommandInterface rr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity"); - - fl_c_v.set_value(0.1); - fr_c_v.set_value(0.1); - rl_c_v.set_value(0.1); - rr_c_v.set_value(0.1); - - ASSERT_FLOAT_EQ(fl_c_v.get_value(), 0.1); - ASSERT_FLOAT_EQ(fr_c_v.get_value(), 0.1); - ASSERT_FLOAT_EQ(rl_c_v.get_value(), 0.1); - ASSERT_FLOAT_EQ(rr_c_v.get_value(), 0.1); - - pth_test_.GetResourceManager()->write(TIME, PERIOD); - - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - - pth_test_.ShutdownPantherSystem(); -} - -// INITIAL PROCEDURE -TEST_F(TestPantherSystem, InitialProcedureTestPantherSystem) -{ - using hardware_interface::LoanedStateInterface; - using panther_hardware_interfaces_test::DriverChannel; - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 234); - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 32); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL1, 54); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetRoboteqCmd(DriverChannel::CHANNEL2, 12); - - pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetResetRoboteqScript(65); - pth_test_.GetRoboteqsMock()->GetRearDriver()->SetResetRoboteqScript(23); - - pth_test_.ConfigureActivatePantherSystem(); - - ASSERT_EQ(pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetResetRoboteqScript(), 2); - ASSERT_EQ(pth_test_.GetRoboteqsMock()->GetRearDriver()->GetResetRoboteqScript(), 2); - - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), 0); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), 0); - - pth_test_.ShutdownPantherSystem(); -} - -// ERROR HANDLING -// TODO: FIX - return code -10, but otherwise seems to work -// TEST(TestPantherSystemOthers, test_error_state) -// { -// using panther_hardware_interfaces_test::kDefaultJoints; -// using panther_hardware_interfaces_test::kDefaultParamMap; -// using panther_hardware_interfaces_test::kPantherSystemName; - -// panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_; - -// auto param_map = kDefaultParamMap; - -// param_map["max_write_pdo_cmds_errors_count"] = "1"; -// param_map["max_read_pdo_motor_states_errors_count"] = "1"; -// param_map["max_read_pdo_driver_state_errors_count"] = "1"; - -// const std::string panther_system_urdf_ = pth_test_.BuildUrdf(param_map, kDefaultJoints); -// const float period_ = 0.01; - -// pth_test_.Start(panther_system_urdf_); - -// pth_test_.ConfigureActivatePantherSystem(); - -// pth_test_.GetRoboteqsMock()->GetFrontDriver()->StopPublishing(); -// pth_test_.GetRoboteqsMock()->GetRearDriver()->StopPublishing(); -// pth_test_.GetRoboteqsMock()->GetFrontDriver()->SetOnWriteWait(0x202C, 0, 200000); -// pth_test_.GetRoboteqsMock()->GetRearDriver()->SetOnWriteWait(0x202C, 0, 200000); - -// auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME); -// const auto PERIOD = rclcpp::Duration::from_seconds(period_); - -// pth_test_.GetResourceManager()->read(TIME, PERIOD); -// pth_test_.GetResourceManager()->write(TIME, PERIOD); - -// auto status_map = pth_test_.GetResourceManager()->get_components_status(); -// ASSERT_EQ( -// status_map[kPantherSystemName].state.label(), -// hardware_interface::lifecycle_state_names::FINALIZED); - -// pth_test_.Stop(); -// } - -// WRONG ORDER URDF -TEST(TestPantherSystemOthers, WrongOrderURDF) -{ - using hardware_interface::LoanedCommandInterface; - - using panther_hardware_interfaces_test::DriverChannel; - - using panther_hardware_interfaces_test::kDefaultParamMap; - using panther_hardware_interfaces_test::kRadPerSecToRbtqCmd; - - const float period_ = 0.01; - - panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_; - - std::vector joints = { - "rr_wheel_joint", "fl_wheel_joint", "fr_wheel_joint", "rl_wheel_joint"}; - - const std::string panther_system_urdf_ = pth_test_.BuildUrdf(kDefaultParamMap, joints); - - pth_test_.Start(panther_system_urdf_); - - const float fl_v = 0.1; - const float fr_v = 0.2; - const float rl_v = 0.3; - const float rr_v = 0.4; - - pth_test_.ConfigureActivatePantherSystem(); - - // loaned command interfaces have to be destroyed before running Stop - { - LoanedCommandInterface fl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fl_wheel_joint/velocity"); - LoanedCommandInterface fr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("fr_wheel_joint/velocity"); - LoanedCommandInterface rl_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rl_wheel_joint/velocity"); - LoanedCommandInterface rr_c_v = - pth_test_.GetResourceManager()->claim_command_interface("rr_wheel_joint/velocity"); - - fl_c_v.set_value(fl_v); - fr_c_v.set_value(fr_v); - rl_c_v.set_value(rl_v); - rr_c_v.set_value(rr_v); - - ASSERT_FLOAT_EQ(fl_c_v.get_value(), fl_v); - ASSERT_FLOAT_EQ(fr_c_v.get_value(), fr_v); - ASSERT_FLOAT_EQ(rl_c_v.get_value(), rl_v); - ASSERT_FLOAT_EQ(rr_c_v.get_value(), rr_v); - - const auto TIME = rclcpp::Time(0); - const auto PERIOD = rclcpp::Duration::from_seconds(period_); - - pth_test_.GetResourceManager()->write(TIME, PERIOD); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(fl_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetFrontDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(fr_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL2), - static_cast(rl_v * kRadPerSecToRbtqCmd)); - ASSERT_EQ( - pth_test_.GetRoboteqsMock()->GetRearDriver()->GetRoboteqCmd(DriverChannel::CHANNEL1), - static_cast(rr_v * kRadPerSecToRbtqCmd)); - } - - pth_test_.ShutdownPantherSystem(); - - pth_test_.Stop(); -} - -// TIMEOUT TESTS - -// TODO: fix -// TEST(TestPantherSystemOthers, pdo_read_motors_states_timeout_test) -// { -// using panther_hardware_interfaces_test::kDefaultJoints; -// using panther_hardware_interfaces_test::kDefaultParamMap; - -// panther_hardware_interfaces_test::PantherSystemTestUtils pth_test_; - -// auto param_map = kDefaultParamMap; - -// // It is necessary to set max_read_pdo_errors_count to some higher value, because -// // adding wait time to Roboteq mock block all communication (also PDO), and PDO timeouts -// // happen -// param_map["pdo_motor_states_timeout_ms"] = "15"; -// param_map["max_write_pdo_cmds_errors_count"] = "100"; -// param_map["max_read_pdo_motor_states_errors_count"] = "2"; -// param_map["max_read_pdo_driver_state_errors_count"] = "2"; - -// const std::string panther_system_urdf_ = pth_test_.BuildUrdf(param_map, kDefaultJoints); -// const float period_ = 0.01; - -// pth_test_.Start(panther_system_urdf_); - -// rclcpp::Node::SharedPtr node = std::make_shared("hardware_interface_test_node"); -// pth_test_.ConfigureActivatePantherSystem(); - -// panther_msgs::msg::DriverState::SharedPtr state_msg; -// auto sub = node->create_subscription( -// panther_hardware_interfaces_test::kMotorControllersStateTopic, rclcpp::SensorDataQoS(), -// [&](const panther_msgs::msg::DriverState::SharedPtr msg) { state_msg = msg; }); - -// std::this_thread::sleep_for(std::chrono::seconds(2)); - -// auto TIME = rclcpp::Time(0, 0, RCL_ROS_TIME); -// const auto PERIOD = rclcpp::Duration::from_seconds(period_); - -// pth_test_.GetResourceManager()->read(TIME, PERIOD); - -// ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); -// ASSERT_FALSE(state_msg->read_pdo_motor_states_error); -// state_msg.reset(); - -// pth_test_.GetRoboteqsMock()->GetFrontDriver()->StopPublishing(); -// pth_test_.GetRoboteqsMock()->GetRearDriver()->StopPublishing(); - -// pth_test_.GetResourceManager()->write(TIME, PERIOD); - -// std::this_thread::sleep_for(PERIOD.to_chrono()); - -// TIME += PERIOD; -// pth_test_.GetResourceManager()->read(TIME, PERIOD); - -// ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); -// ASSERT_FALSE(state_msg->read_pdo_motor_states_error); -// state_msg.reset(); - -// pth_test_.GetResourceManager()->write(TIME, PERIOD); - -// std::this_thread::sleep_for(PERIOD.to_chrono()); - -// TIME += PERIOD; -// pth_test_.GetResourceManager()->read(TIME, PERIOD); - -// ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node, state_msg, std::chrono::seconds(5))); -// ASSERT_TRUE(state_msg->read_pdo_motor_states_error); - -// pth_test_.ShutdownPantherSystem(); - -// pth_test_.Stop(); -// } - -// todo E-stop tests - it will the best to add them along with GPIO, as it will change the E-stop -// procedure - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - - // For testing individual tests: - // testing::GTEST_FLAG(filter) = "TestPantherSystemOthers.pdo_read_motors_states_timeout_test"; - - return RUN_ALL_TESTS(); -} diff --git a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp b/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp deleted file mode 100644 index 7389a322..00000000 --- a/panther_hardware_interfaces/test/panther_system/test_panther_system_ros_interface.cpp +++ /dev/null @@ -1,276 +0,0 @@ -// Copyright 2023 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 - -#include - -#include - -#include - -#include "utils/test_constants.hpp" - -class TestPantherSystemRosInterface : public ::testing::Test -{ -public: - TestPantherSystemRosInterface() - { - using panther_hardware_interfaces::PantherSystemRosInterface; - - test_node_ = std::make_shared("test_panther_system_node"); - - driver_state_sub_ = test_node_->create_subscription( - panther_hardware_interfaces_test::kMotorControllersStateTopic, rclcpp::SensorDataQoS(), - [&](const panther_msgs::msg::DriverState::SharedPtr msg) { driver_state_msg_ = msg; }); - - panther_system_ros_interface_ = - std::make_unique("hardware_controller"); - } - - ~TestPantherSystemRosInterface() { panther_system_ros_interface_.reset(); } - -protected: - rclcpp::Node::SharedPtr test_node_; - rclcpp::Subscription::SharedPtr driver_state_sub_; - panther_msgs::msg::DriverState::SharedPtr driver_state_msg_; - std::unique_ptr - panther_system_ros_interface_; -}; - -TEST(TestPantherSystemRosInterfaceInitialization, NodeCreation) -{ - using panther_hardware_interfaces::PantherSystemRosInterface; - - std::vector node_names; - const std::string panther_system_node_name = "hardware_controller"; - const std::string panther_system_node_name_with_ns = "/" + panther_system_node_name; - - rclcpp::Node::SharedPtr test_node = std::make_shared("test_panther_system_node"); - - std::unique_ptr panther_system_ros_interface; - - panther_system_ros_interface = - std::make_unique(panther_system_node_name); - node_names = test_node->get_node_names(); - ASSERT_TRUE( - std::find(node_names.begin(), node_names.end(), panther_system_node_name_with_ns) != - node_names.end()); - - panther_system_ros_interface.reset(); - node_names = test_node->get_node_names(); - ASSERT_FALSE( - std::find(node_names.begin(), node_names.end(), panther_system_node_name_with_ns) != - node_names.end()); - - // Check if it is possible to create a node once again (if everything was cleaned up properly) - panther_system_ros_interface = - std::make_unique(panther_system_node_name); - node_names = test_node->get_node_names(); - ASSERT_TRUE( - std::find(node_names.begin(), node_names.end(), panther_system_node_name_with_ns) != - node_names.end()); - - panther_system_ros_interface.reset(); -} - -TEST(TestPantherSystemRosInterfaceInitialization, Activation) -{ - using panther_hardware_interfaces::PantherSystemRosInterface; - using panther_hardware_interfaces_test::kMotorControllersStateTopic; - - std::map> service_names_and_types; - std::map> topic_names_and_types; - - rclcpp::Node::SharedPtr test_node = std::make_shared("test_panther_system_node"); - - std::unique_ptr panther_system_ros_interface = - std::make_unique("hardware_controller"); - - // Necessary to add some waiting, so that topic lists are updated - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - topic_names_and_types = test_node->get_topic_names_and_types(); - ASSERT_TRUE( - topic_names_and_types.find(kMotorControllersStateTopic) != topic_names_and_types.end()); - - panther_system_ros_interface.reset(); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - topic_names_and_types = test_node->get_topic_names_and_types(); - ASSERT_FALSE( - topic_names_and_types.find(kMotorControllersStateTopic) != topic_names_and_types.end()); - - panther_system_ros_interface = std::make_unique("hardware_controller"); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - topic_names_and_types = test_node->get_topic_names_and_types(); - ASSERT_TRUE( - topic_names_and_types.find(kMotorControllersStateTopic) != topic_names_and_types.end()); - - panther_system_ros_interface.reset(); -} - -TEST_F(TestPantherSystemRosInterface, ErrorFlags) -{ - panther_hardware_interfaces::RoboteqData front( - panther_hardware_interfaces_test::kDrivetrainSettings); - panther_hardware_interfaces::RoboteqData rear( - panther_hardware_interfaces_test::kDrivetrainSettings); - - panther_hardware_interfaces::RoboteqDriverState front_driver_state; - front_driver_state.fault_flags = 0b00000001; - front_driver_state.script_flags = 0b00000010; - front_driver_state.runtime_stat_flag_motor_1 = 0b00001000; - front_driver_state.runtime_stat_flag_motor_2 = 0b00000100; - - panther_hardware_interfaces::RoboteqDriverState rear_driver_state; - rear_driver_state.fault_flags = 0b00000010; - rear_driver_state.script_flags = 0b00000001; - rear_driver_state.runtime_stat_flag_motor_1 = 0b00100000; - rear_driver_state.runtime_stat_flag_motor_2 = 0b00010000; - - front.SetDriverState(front_driver_state, false); - rear.SetDriverState(rear_driver_state, false); - - panther_system_ros_interface_->UpdateMsgErrorFlags(front, rear); - panther_system_ros_interface_->PublishDriverState(); - - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5))); - - EXPECT_TRUE(driver_state_msg_->front.fault_flag.overheat); - EXPECT_TRUE(driver_state_msg_->front.script_flag.encoder_disconnected); - EXPECT_TRUE(driver_state_msg_->front.left_motor_runtime_error.loop_error); - EXPECT_TRUE(driver_state_msg_->front.right_motor_runtime_error.safety_stop_active); - - EXPECT_TRUE(driver_state_msg_->rear.fault_flag.overvoltage); - EXPECT_TRUE(driver_state_msg_->rear.script_flag.loop_error); - EXPECT_TRUE(driver_state_msg_->rear.left_motor_runtime_error.forward_limit_triggered); - EXPECT_TRUE(driver_state_msg_->rear.right_motor_runtime_error.reverse_limit_triggered); -} - -TEST_F(TestPantherSystemRosInterface, DriversStates) -{ - panther_hardware_interfaces::DriverState front; - panther_hardware_interfaces::DriverState rear; - - const std::int16_t f_temp = 36; - const std::int16_t f_heatsink_temp = 37; - const std::uint16_t f_volt = 405; - const std::int16_t f_battery_current_1 = 15; - const std::int16_t f_battery_current_2 = 12; - const std::int16_t r_temp = 35; - const std::int16_t r_heatsink_temp = 36; - const std::uint16_t r_volt = 404; - const std::int16_t r_battery_current_1 = 14; - const std::int16_t r_battery_current_2 = 11; - - front.SetTemperature(f_temp); - front.SetHeatsinkTemperature(f_heatsink_temp); - front.SetVoltage(f_volt); - front.SetBatteryCurrent1(f_battery_current_1); - front.SetBatteryCurrent2(f_battery_current_2); - - rear.SetTemperature(r_temp); - rear.SetHeatsinkTemperature(r_heatsink_temp); - rear.SetVoltage(r_volt); - rear.SetBatteryCurrent1(r_battery_current_1); - rear.SetBatteryCurrent2(r_battery_current_2); - - panther_system_ros_interface_->UpdateMsgDriversStates(front, rear); - panther_system_ros_interface_->PublishDriverState(); - - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5))); - - EXPECT_FLOAT_EQ(static_cast(driver_state_msg_->front.temperature), f_temp); - EXPECT_FLOAT_EQ(static_cast(driver_state_msg_->rear.temperature), r_temp); - - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->front.heatsink_temperature), f_heatsink_temp); - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->rear.heatsink_temperature), r_heatsink_temp); - - EXPECT_FLOAT_EQ(static_cast(driver_state_msg_->front.voltage * 10.0), f_volt); - EXPECT_FLOAT_EQ(static_cast(driver_state_msg_->rear.voltage * 10.0), r_volt); - - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->front.current * 10.0), - (f_battery_current_1 + f_battery_current_2)); - EXPECT_FLOAT_EQ( - static_cast(driver_state_msg_->rear.current * 10.0), - (r_battery_current_1 + r_battery_current_2)); -} - -TEST_F(TestPantherSystemRosInterface, Errors) -{ - panther_hardware_interfaces::CANErrors can_errors; - can_errors.error = true; - - can_errors.write_pdo_cmds_error = true; - can_errors.read_pdo_motor_states_error = false; - can_errors.read_pdo_driver_state_error = false; - - can_errors.front_motor_states_data_timed_out = true; - can_errors.rear_motor_states_data_timed_out = false; - - can_errors.front_driver_state_data_timed_out = false; - can_errors.rear_driver_state_data_timed_out = true; - - can_errors.front_can_error = false; - can_errors.rear_can_error = true; - - panther_system_ros_interface_->UpdateMsgErrors(can_errors); - - panther_system_ros_interface_->PublishDriverState(); - - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(test_node_, driver_state_msg_, std::chrono::seconds(5))); - - EXPECT_TRUE(driver_state_msg_->error); - - EXPECT_TRUE(driver_state_msg_->write_pdo_cmds_error); - EXPECT_FALSE(driver_state_msg_->read_pdo_motor_states_error); - EXPECT_FALSE(driver_state_msg_->read_pdo_driver_state_error); - - EXPECT_TRUE(driver_state_msg_->front.motor_states_data_timed_out); - EXPECT_FALSE(driver_state_msg_->rear.motor_states_data_timed_out); - - EXPECT_FALSE(driver_state_msg_->front.driver_state_data_timed_out); - EXPECT_TRUE(driver_state_msg_->rear.driver_state_data_timed_out); - - EXPECT_FALSE(driver_state_msg_->front.can_error); - EXPECT_TRUE(driver_state_msg_->rear.can_error); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - testing::InitGoogleTest(&argc, argv); - - auto run_tests = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - return run_tests; -} diff --git a/panther_hardware_interfaces/test/utils/panther_system_test_utils.hpp b/panther_hardware_interfaces/test/utils/panther_system_test_utils.hpp deleted file mode 100644 index 0e07cc37..00000000 --- a/panther_hardware_interfaces/test/utils/panther_system_test_utils.hpp +++ /dev/null @@ -1,164 +0,0 @@ -// Copyright 2023 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_HARDWARE_INTERFACES_TEST_UTILS_PANTHER_SYSTEM_TEST_UTILS_HPP_ -#define PANTHER_HARDWARE_INTERFACES_TEST_UTILS_PANTHER_SYSTEM_TEST_UTILS_HPP_ - -#include -#include -#include -#include - -#include - -#include -#include - -#include "roboteqs_mock.hpp" -#include "test_constants.hpp" - -namespace panther_hardware_interfaces_test -{ - -/** - * @brief Utility class for testing Panther System - */ -class PantherSystemTestUtils -{ -public: - PantherSystemTestUtils() - { - default_panther_system_urdf_ = BuildUrdf(kDefaultParamMap, kDefaultJoints); - } - - /** - * @brief Starts Roboteq Mock, initializes rclcpp and creates resource manager - * @param urdf urdf used to create resource manager - */ - void Start(const std::string & urdf) - { - roboteqs_mock_ = std::make_unique(); - roboteqs_mock_->Start(std::chrono::milliseconds(10), std::chrono::milliseconds(50)); - rclcpp::init(0, nullptr); - - rm_ = std::make_shared(urdf); - } - - /** - * @brief Shuts down rclcpp, stops Roboteq mock and destroys resource manager - */ - void Stop() - { - rclcpp::shutdown(); - roboteqs_mock_->Stop(); - roboteqs_mock_.reset(); - rm_.reset(); - } - - /** - * @brief Creates and returns URDF as a string - * @param param_map map with hardware parameters - * @param joints vector of joint names - */ - std::string BuildUrdf( - const std::map & param_map, const std::vector & joints) - { - std::stringstream urdf; - - urdf << kUrdfHeader << "" << std::endl << kPluginName; - - for (auto const & [key, val] : param_map) { - urdf << "" << val << "" << std::endl; - } - - urdf << "" << std::endl; - - for (auto const & joint : joints) { - urdf << "" << std::endl - << kJointInterfaces << "" << std::endl; - } - - urdf << kUrdfFooter; - - return urdf.str(); - } - - void ConfigurePantherSystem() - { - SetState( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - } - - void UnconfigurePantherSystem() - { - SetState( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - hardware_interface::lifecycle_state_names::UNCONFIGURED); - } - - void ActivatePantherSystem() - { - SetState( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); - } - - void DeactivatePantherSystem() - { - SetState( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - hardware_interface::lifecycle_state_names::INACTIVE); - } - - void ShutdownPantherSystem() - { - SetState( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, - hardware_interface::lifecycle_state_names::FINALIZED); - } - - void ConfigureActivatePantherSystem() - { - ConfigurePantherSystem(); - ActivatePantherSystem(); - } - - std::shared_ptr GetResourceManager() { return rm_; } - std::shared_ptr GetRoboteqsMock() { return roboteqs_mock_; } - - std::string GetDefaultPantherSystemUrdf() const { return default_panther_system_urdf_; } - -private: - /** - * @brief Changes current state of the resource manager to the one set in parameters. It is - * recommended to use wrapper functions - * @param state_id - * @param state_name - */ - void SetState(const std::uint8_t state_id, const std::string & state_name) - { - rclcpp_lifecycle::State state(state_id, state_name); - rm_->set_component_state(kPantherSystemName, state); - } - - std::shared_ptr roboteqs_mock_; - std::shared_ptr rm_; - - std::string default_panther_system_urdf_; -}; - -} // namespace panther_hardware_interfaces_test - -#endif // PANTHER_HARDWARE_INTERFACES_TEST_UTILS_PANTHER_SYSTEM_TEST_UTILS_HPP_ diff --git a/panther_lights/.docs/CHARGING_BATTERY.webp b/panther_lights/.docs/CHARGING_BATTERY.webp deleted file mode 100644 index 8a5e8238..00000000 Binary files a/panther_lights/.docs/CHARGING_BATTERY.webp and /dev/null differ diff --git a/panther_lights/.docs/ERROR.webp b/panther_lights/.docs/ERROR.webp deleted file mode 100644 index c5628983..00000000 Binary files a/panther_lights/.docs/ERROR.webp and /dev/null differ diff --git a/panther_lights/.docs/GOAL_ACHIEVED.webp b/panther_lights/.docs/GOAL_ACHIEVED.webp deleted file mode 100644 index f194ba38..00000000 Binary files a/panther_lights/.docs/GOAL_ACHIEVED.webp and /dev/null differ diff --git a/panther_lights/plugins.xml b/panther_lights/plugins.xml deleted file mode 100644 index 11f0ed75..00000000 --- a/panther_lights/plugins.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - Animation processed from an image - - - Charging animation representing percentage of battery - -