From 4ccf48f706e2665c37b185f08c7b980644e22342 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 18 Jun 2024 07:59:01 +0000 Subject: [PATCH 01/67] Implement testing POC --- panther_diagnostics/CMakeLists.txt | 10 ++- .../panther_diagnostics/filesystem.hpp | 78 +++++++++++++++++++ .../system_status_node.hpp | 21 +++-- .../include/panther_diagnostics/types.hpp | 40 ++++++++++ panther_diagnostics/package.xml | 2 + panther_diagnostics/src/main.cpp | 6 +- .../src/system_status_node.cpp | 30 ++++--- .../integration/system_status_node.test.py | 72 +++++++++++++++++ .../{ => unit}/test_system_status_node.cpp | 44 +++++++++-- 9 files changed, 268 insertions(+), 35 deletions(-) create mode 100644 panther_diagnostics/include/panther_diagnostics/filesystem.hpp create mode 100644 panther_diagnostics/include/panther_diagnostics/types.hpp create mode 100644 panther_diagnostics/test/integration/system_status_node.test.py rename panther_diagnostics/test/{ => unit}/test_system_status_node.cpp (74%) diff --git a/panther_diagnostics/CMakeLists.txt b/panther_diagnostics/CMakeLists.txt index 6c73bb2f4..ecda443aa 100644 --- a/panther_diagnostics/CMakeLists.txt +++ b/panther_diagnostics/CMakeLists.txt @@ -53,10 +53,12 @@ install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) install(TARGETS system_status_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(ros_testing REQUIRED) - ament_add_gtest(${PROJECT_NAME}_test_system_status - test/test_system_status_node.cpp src/system_status_node.cpp) + ament_add_gmock( + ${PROJECT_NAME}_test_system_status test/unit/test_system_status_node.cpp + src/system_status_node.cpp) target_include_directories(${PROJECT_NAME}_test_system_status PUBLIC ${CMAKE_INSTALL_PREFIX}/include) @@ -65,6 +67,8 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_system_status system_status_parameters PkgConfig::CPPUPROFILE) + add_ros_test(test/integration/system_status_node.test.py) + endif() ament_package() diff --git a/panther_diagnostics/include/panther_diagnostics/filesystem.hpp b/panther_diagnostics/include/panther_diagnostics/filesystem.hpp new file mode 100644 index 000000000..527971267 --- /dev/null +++ b/panther_diagnostics/include/panther_diagnostics/filesystem.hpp @@ -0,0 +1,78 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_DIAGNOSTICS__FILESYSTEM_HPP_ +#define PANTHER_DIAGNOSTICS__FILESYSTEM_HPP_ + +#include + +namespace panther_diagnostics +{ + +/** + * @brief Interface for interacting with the filesystem. + * + * This interface provides a way to interact with the filesystem, allowing users to retrieve space + * information for a given path. + */ +class FilesystemInterface +{ +public: + /** + * @brief Virtual destructor for the FilesystemInterface class. + */ + virtual ~FilesystemInterface() = default; + + /** + * @brief Retrieves space information for a given path. + * + * @param path The path for which to retrieve space information. + * @return std::filesystem::space_info The space information for the given path. + */ + virtual std::filesystem::space_info GetSpaceInfo(const std::filesystem::path & path) const = 0; + + /** + * @brief Alias for a shared pointer to a FilesystemInterface object. + * + */ + using SharedPtr = std::shared_ptr; +}; + +/** + * @brief The Filesystem class provides functionality related to filesystem operations. + * + * This class inherits from the FilesystemInterface and provides an implementation for + * getting space information for a given path. + */ +class Filesystem : public FilesystemInterface +{ +public: + /** + * @brief Get the space information for a given path. + * + * This function returns the space information (capacity, free space, and available space) + * for the specified path. + * + * @param path The path for which to retrieve the space information. + * @return The space information for the specified path. + */ + inline std::filesystem::space_info GetSpaceInfo(const std::filesystem::path & path) const override + { + return std::filesystem::space(path); + } +}; + +} // namespace panther_diagnostics + +#endif // PANTHER_DIAGNOSTICS__FILESYSTEM_HPP_ diff --git a/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp b/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp index 235d5d417..e5211f486 100644 --- a/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp +++ b/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp @@ -17,13 +17,16 @@ #include -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include #include "panther_msgs/msg/system_status.hpp" #include "system_status_parameters.hpp" +#include "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/types.hpp" + using namespace std::chrono_literals; namespace panther_diagnostics @@ -32,19 +35,11 @@ namespace panther_diagnostics class SystemStatusNode : public rclcpp::Node { public: - SystemStatusNode(const std::string & node_name); - - struct SystemStatus - { - std::vector core_usages; - float cpu_mean_usage; - float cpu_temperature; - float memory_usage; - float disk_usage; - }; + SystemStatusNode(const std::string & node_name, FilesystemInterface::SharedPtr filesystem); protected: SystemStatus GetSystemStatus() const; + std::vector GetCoresUsages() const; float GetCPUMeanUsage(const std::vector & usages) const; float GetCPUTemperature(const std::string & filename) const; @@ -57,6 +52,8 @@ class SystemStatusNode : public rclcpp::Node void TimerCallback(); void DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapper & status); + FilesystemInterface::SharedPtr filesystem_; + rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr system_status_publisher_; diagnostic_updater::Updater diagnostic_updater_; diff --git a/panther_diagnostics/include/panther_diagnostics/types.hpp b/panther_diagnostics/include/panther_diagnostics/types.hpp new file mode 100644 index 000000000..b636d22af --- /dev/null +++ b/panther_diagnostics/include/panther_diagnostics/types.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_DIAGNOSTICS__TYPES_HPP_ +#define PANTHER_DIAGNOSTICS__TYPES_HPP_ + +#include + +namespace panther_diagnostics +{ + +/** + * @brief Represents the system status information. + * + * This struct contains various metrics related to the system status, such as CPU usage, CPU + * temperature, memory usage, and disk usage. + */ +struct SystemStatus +{ + std::vector core_usages; + float cpu_mean_usage; + float cpu_temperature; + float memory_usage; + float disk_usage; +}; + +} // namespace panther_diagnostics + +#endif // PANTHER_DIAGNOSTICS__TYPES_HPP_ diff --git a/panther_diagnostics/package.xml b/panther_diagnostics/package.xml index fff9d18f7..163250486 100644 --- a/panther_diagnostics/package.xml +++ b/panther_diagnostics/package.xml @@ -25,6 +25,8 @@ std_msgs ament_cmake_gtest + google-mock + ros_testing ament_cmake diff --git a/panther_diagnostics/src/main.cpp b/panther_diagnostics/src/main.cpp index f9ffdb280..544eb3aae 100644 --- a/panther_diagnostics/src/main.cpp +++ b/panther_diagnostics/src/main.cpp @@ -18,14 +18,16 @@ #include "rclcpp/rclcpp.hpp" +#include "panther_diagnostics/filesystem.hpp" #include "panther_diagnostics/system_status_node.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto system_status_node = - std::make_shared("system_status_node"); + auto filesystem = std::make_shared(); + auto system_status_node = std::make_shared( + "system_status_node", filesystem); try { rclcpp::spin(system_status_node); diff --git a/panther_diagnostics/src/system_status_node.cpp b/panther_diagnostics/src/system_status_node.cpp index 91edf7ed4..55e9c3a60 100644 --- a/panther_diagnostics/src/system_status_node.cpp +++ b/panther_diagnostics/src/system_status_node.cpp @@ -19,19 +19,24 @@ #include #include -#include "cppuprofile/uprofile.h" +#include -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include #include "panther_msgs/msg/system_status.hpp" + #include "panther_utils/common_utilities.hpp" +#include "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/types.hpp" + namespace panther_diagnostics { -SystemStatusNode::SystemStatusNode(const std::string & node_name) -: rclcpp::Node(node_name), diagnostic_updater_(this) +SystemStatusNode::SystemStatusNode( + const std::string & node_name, FilesystemInterface::SharedPtr filesystem) +: rclcpp::Node(node_name), diagnostic_updater_(this), filesystem_(filesystem) { RCLCPP_INFO(this->get_logger(), "Initializing."); @@ -41,6 +46,7 @@ SystemStatusNode::SystemStatusNode(const std::string & node_name) system_status_publisher_ = this->create_publisher( "system_status", 10); + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(params_.publish_rate * 1000)), std::bind(&SystemStatusNode::TimerCallback, this)); @@ -59,9 +65,9 @@ void SystemStatusNode::TimerCallback() system_status_publisher_->publish(message); } -SystemStatusNode::SystemStatus SystemStatusNode::GetSystemStatus() const +SystemStatus SystemStatusNode::GetSystemStatus() const { - SystemStatusNode::SystemStatus status; + SystemStatus status; status.core_usages = GetCoresUsages(); status.cpu_mean_usage = GetCPUMeanUsage(status.core_usages); @@ -109,13 +115,15 @@ float SystemStatusNode::GetMemoryUsage() const float SystemStatusNode::GetDiskUsage() const { const std::filesystem::directory_entry entry("/"); - const std::filesystem::space_info si = std::filesystem::space(entry.path()); + const auto space_info = filesystem_->GetSpaceInfo(entry.path()); + + const auto disk_usage = static_cast(space_info.capacity - space_info.available) / + space_info.capacity * 100.0; - return static_cast(si.capacity - si.available) / si.capacity * 100.0; + return disk_usage; } -panther_msgs::msg::SystemStatus SystemStatusNode::SystemStatusToMessage( - const SystemStatusNode::SystemStatus & status) +panther_msgs::msg::SystemStatus SystemStatusNode::SystemStatusToMessage(const SystemStatus & status) { panther_msgs::msg::SystemStatus message; diff --git a/panther_diagnostics/test/integration/system_status_node.test.py b/panther_diagnostics/test/integration/system_status_node.test.py new file mode 100644 index 000000000..6f79ff43b --- /dev/null +++ b/panther_diagnostics/test/integration/system_status_node.test.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import unittest + +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing_ros import WaitForTopics +import launch_testing +import pytest + +from panther_msgs.msg import SystemStatus +from diagnostic_msgs.msg import DiagnosticArray + + +@pytest.mark.launch_test +def generate_test_description(): + + system_status_node = Node( + package="panther_diagnostics", + executable="system_status_node", + ) + + context = {} + + return ( + LaunchDescription( + [ + system_status_node, + # Start test after 1s + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestNodeInitialization(unittest.TestCase): + def test_initialization_log(self, proc_output): + proc_output.assertWaitFor("Initialized successfully.") + + +class TestNode(unittest.TestCase): + def test_msg(self): + topic_list = [('system_status', SystemStatus), ('diagnostics', DiagnosticArray)] + + with WaitForTopics(topic_list, timeout=5.0) as wait_for_topics: + received_topics_str = ', '.join(wait_for_topics.topics_received()) + print("Received messages from the following topics: [" + received_topics_str + "]") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/panther_diagnostics/test/test_system_status_node.cpp b/panther_diagnostics/test/unit/test_system_status_node.cpp similarity index 74% rename from panther_diagnostics/test/test_system_status_node.cpp rename to panther_diagnostics/test/unit/test_system_status_node.cpp index 9f606cc55..059b922d0 100644 --- a/panther_diagnostics/test/test_system_status_node.cpp +++ b/panther_diagnostics/test/unit/test_system_status_node.cpp @@ -17,14 +17,27 @@ #include #include +#include "gmock/gmock.h" #include "gtest/gtest.h" +#include "panther_diagnostics/filesystem.hpp" #include "panther_diagnostics/system_status_node.hpp" +class MockFilesystem : public panther_diagnostics::FilesystemInterface +{ +public: + MOCK_METHOD( + std::filesystem::space_info, GetSpaceInfo, (const std::filesystem::path & path), + (const, override)); +}; + class SystemStatusNodeWrapper : public panther_diagnostics::SystemStatusNode { public: - SystemStatusNodeWrapper() : panther_diagnostics::SystemStatusNode("test_system_statics") {} + SystemStatusNodeWrapper(MockFilesystem::SharedPtr filesystem) + : panther_diagnostics::SystemStatusNode("test_system_statics", filesystem) + { + } std::vector GetCoresUsages() const { @@ -46,7 +59,7 @@ class SystemStatusNodeWrapper : public panther_diagnostics::SystemStatusNode float GetDiskUsage() const { return panther_diagnostics::SystemStatusNode::GetDiskUsage(); } panther_msgs::msg::SystemStatus SystemStatusToMessage( - const panther_diagnostics::SystemStatusNode::SystemStatus & status) + const panther_diagnostics::SystemStatus & status) { return panther_diagnostics::SystemStatusNode::SystemStatusToMessage(status); } @@ -58,12 +71,14 @@ class TestSystemStatusNode : public testing::Test TestSystemStatusNode(); protected: + MockFilesystem::SharedPtr filesystem_; std::unique_ptr system_status_; }; TestSystemStatusNode::TestSystemStatusNode() { - system_status_ = std::make_unique(); + filesystem_ = std::make_shared(); + system_status_ = std::make_unique(filesystem_); } TEST_F(TestSystemStatusNode, CheckCoresUsages) @@ -107,16 +122,31 @@ TEST_F(TestSystemStatusNode, CheckMemoryReadings) EXPECT_TRUE((memory >= 0.0) && (memory <= 100.0)); } -TEST_F(TestSystemStatusNode, CheckDiskReadings) +// TEST_F(TestSystemStatusNode, CheckDiskReadings) +// { +// const auto disk_usage = system_status_->GetDiskUsage(); + +// EXPECT_TRUE((disk_usage >= 0.0) && (disk_usage <= 100.0)); +// } + +TEST(TestDiskUsage, RegularConsumption) { - const auto disk_usage = system_status_->GetDiskUsage(); + auto filesystem_mock = std::make_shared(); + auto system_status = std::make_unique(filesystem_mock); + + std::filesystem::space_info space_info{1000, 500, 500}; + std::filesystem::path path = "/"; + + EXPECT_CALL(*filesystem_mock, GetSpaceInfo(path)).WillOnce(::testing::Return(space_info)); + + auto usage = system_status->GetDiskUsage(); - EXPECT_TRUE((disk_usage >= 0.0) && (disk_usage <= 100.0)); + EXPECT_EQ(usage, 50.0); } TEST_F(TestSystemStatusNode, CheckSystemStatusToMessage) { - panther_diagnostics::SystemStatusNode::SystemStatus status; + panther_diagnostics::SystemStatus status; status.core_usages = {50.0, 50.0, 50.0}; status.cpu_mean_usage = 50.0; status.cpu_temperature = 36.6; From 25da047db3313f70b0d19382d86c27155ba75b55 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 25 Jul 2024 18:18:41 +0200 Subject: [PATCH 02/67] Namespace refactor --- panther_description/urdf/gazebo.urdf.xacro | 14 ++------------ panther_description/urdf/panther_macro.urdf.xacro | 7 +------ 2 files changed, 3 insertions(+), 18 deletions(-) diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro index b8b2afdbb..4d315fd29 100644 --- a/panther_description/urdf/gazebo.urdf.xacro +++ b/panther_description/urdf/gazebo.urdf.xacro @@ -6,12 +6,7 @@ - - - - - - + @@ -65,12 +60,7 @@ - - - - - - + diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro index 7ff4192e2..01c2764bf 100644 --- a/panther_description/urdf/panther_macro.urdf.xacro +++ b/panther_description/urdf/panther_macro.urdf.xacro @@ -16,12 +16,7 @@ imu_rot_y:=0.0 namespace:=''"> - - - - - - + From c96d02036509ddc0119a742db4670a8249315a05 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Mon, 5 Aug 2024 09:53:37 +0200 Subject: [PATCH 03/67] New format of documentation (#369) * Change 3 package for demo * Improve ROS_API * fix links * Update * Update * Table improvements * Format * Save work * Save work * update * fix * fix * fix * fix * fix * Add API warning * Improve links * lights simplify * Create CONFIGURATION.md files * Typos * pre-commit * Apply suggestions from code review Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Save work * Final unification * Delete trash * typos * Update README.md * Update ROS_API.md * Update ROS_API.md * Update README.md Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Change initial warning to beta warning * improve warn rendering * rendering * Update Diagram * Add Dawid suggestions * Dot * Change diagram ext and typos * Do not describe external nodes * Add Dawid suggestons * Add last Dawid suggestions * Format * Pawel suggestions * Diagram improvements * Update * Diagram Visual --------- Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> --- .docs/panther_ros2_api.drawio.svg | 4 + .pre-commit-config.yaml | 2 +- README.md | 53 +++ ROS_API.md | 122 ++++++ panther_battery/README.md | 65 +--- panther_bringup/CMakeLists.txt | 2 +- panther_bringup/README.md | 159 +------- panther_bringup/launch/bringup.launch.py | 4 +- panther_controller/CONFIGURATION.md | 9 + panther_controller/README.md | 121 +----- .../launch/controller.launch.py | 8 +- panther_description/README.md | 11 + .../launch/load_urdf.launch.py | 8 +- panther_diagnostics/README.md | 39 +- panther_gazebo/CONFIGURATION.md | 61 +++ panther_gazebo/README.md | 100 +---- .../launch/simulate_multiple_robots.launch.py | 1 + .../launch/simulate_robot.launch.py | 2 +- panther_hardware_interfaces/README.md | 92 ++--- panther_lights/CONFIGURATION.md | 167 ++++++++ panther_lights/README.md | 277 ++------------ panther_localization/README.md | 50 +-- .../launch/localization.launch.py | 4 +- panther_manager/CONFIGURATION.md | 211 ++++++++++ panther_manager/README.md | 359 +++--------------- .../config/shutdown_hosts.yaml | 0 panther_manager/launch/manager_bt.launch.py | 2 +- 27 files changed, 815 insertions(+), 1118 deletions(-) create mode 100644 .docs/panther_ros2_api.drawio.svg create mode 100644 ROS_API.md create mode 100644 panther_controller/CONFIGURATION.md create mode 100644 panther_gazebo/CONFIGURATION.md create mode 100644 panther_lights/CONFIGURATION.md create mode 100644 panther_manager/CONFIGURATION.md rename {panther_bringup => panther_manager}/config/shutdown_hosts.yaml (100%) diff --git a/.docs/panther_ros2_api.drawio.svg b/.docs/panther_ros2_api.drawio.svg new file mode 100644 index 000000000..cdee93061 --- /dev/null +++ b/.docs/panther_ros2_api.drawio.svg @@ -0,0 +1,4 @@ + + + +
panther_localization
panther_localization
panther_controller
panther_controller
panther_lights
panther_lights
panther_hardware_interfaces
panther_hardware_interfaces
panther_battery
panther_battery
panther_manager
panther_manager
panther_diagnostics
panther_diagnostics
PantherSystem
PantherSystem
hardware_controller
hardware_controller
GPIOController
GPIOController
MotorsController
MotorsController
CANopenController
CANopenController
RoboteqDriver
RoboteqDriver
CAN
CAN
EStopManager
EStopManager
battery_driver
battery_driver
RPi GPIO
RPi GPIO
ADC
ADC
imu_broadcaster
imu_broadcaster
joint_state_broadcaster
joint_state_broadcaster
drive_controller
drive_controller
controller_manager
controller_manager
PantherImuSensor

PantherImuSensor
IMU
IMU
ekf_filter
ekf_filter
ImuFilter
ImuFilter
Spatial
Spatial
imu/data
imu/data
odometry/wheels
odometry/wheels
odometry/filtered
odometry/filtered
joint_states
joint_states
cmd_vel
cmd_vel
dynamic_joint_states
dynamic_joint_states
 hardware/e_stop
 hardware/e_stop
hardware/motor_controllers_state
hardware/motor_controllers_state
 hardware/io_state
 hardware/io_state
hardware/e_stop_trigger
hardware/e_stop_trigger
hardware/e_stop_reset
hardware/e_stop_reset
hardware/<hw_component>_enable
hardware/<hw_component>_enable
robot_state_publisher
robot_state_publisher
robot_description
robot_description
/tf
/tf
/tf_static
/tf_static
/tf
/tf
battery/battery_status
battery/battery_status
Bumper Lights
Bumper Lights
lights_driver
lights_driver
lights_controller
lights_controller
lights/channel_1_frame
lights/channel_1_frame
lights/channel_2_frame
lights/channel_2_frame
lights/set_animation
lights/set_animation
lights/set_brightness
lights/set_brightness
safety_manager
safety_manager
system_monitor
system_monitor
system_status
system_status
lights_manager
lights_manager
hardware/led_control_enable
hardware/led_control_enable
/tf
/tf
battery/charging_status
battery/charging_status
GPIODriver
GPIODriver
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
Legend:
Legend:
Nodes
Nodes
Plugin
Plugin
Objects
Objects
Topics
Topics
Services
Services
Data Transfer
Data Trans...Text is not SVG - cannot display diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 66c65de5b..421c74955 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -51,7 +51,7 @@ repos: "--ignore-words-list", "ned" # north, east, down (NED) ] - exclude_types: [rst] + exclude_types: [rst, svg] language: python types: [text] diff --git a/README.md b/README.md index f738d8fc4..1e2ce7440 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,11 @@ ROS 2 packages for Panther autonomous mobile robot [![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit)](https://github.com/pre-commit/pre-commit) + + + Panther preview + + ## Quick start ### Create workspace @@ -66,6 +71,54 @@ Simulation: ros2 launch panther_gazebo simulation.launch.py ``` +### Launch Arguments + +Launch arguments are largely common to both simulation and physical robot. However, there is a group of arguments that apply only to hardware or only to the simulator. Below is a legend to the tables with all launch arguments. + +| Symbol | Meaning | +| ------ | ---------------------------- | +| 🤖 | Available for physical robot | +| 🖥️ | Available in simulation | + +| | Argument | Description
***Type:*** `Default` | +| --- | ---------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🖥️ | `add_wheel_joints` | Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information.
***bool:*** `False` | +| 🖥️ | `add_world_transform` | Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots).
***bool:*** `False` | +| 🖥️ | `battery_config_path` | Path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only.
***string:*** `None` | +| 🤖🖥️ | `components_config_path` | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options).
***string:*** [`components.yaml`](./panther_description/config/components.yaml) | +| 🤖🖥️ | `controller_config_path` | Path to controller configuration file. A path to custom configuration can be specified here.
***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/) | +| 🤖 | `disable_manager` | Enable or disable manager_bt_node.
***bool:*** `False` | +| 🤖🖥️ | `fuse_gps` | Include GPS for data fusion.
***bool:*** `False` | +| 🖥️ | `gz_bridge_config_path` | Path to the parameter_bridge configuration file.
***string:*** [`gz_bridge.yaml`](./panther_gazebo/config/gz_bridge.yaml) | +| 🖥️ | `gz_gui` | Run simulation with specific GUI layout.
***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config) | +| 🖥️ | `gz_headless_mode` | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations.
***bool:*** `False` | +| 🖥️ | `gz_log_level` | Adjust the level of console output.
***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`) | +| 🖥️ | `gz_world` | Absolute path to SDF world file.
***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf) | +| 🤖🖥️ | `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.yaml`](./panther_manager/config/shutdown_hosts.yaml) | +| 🤖🖥️ | `use_ekf` | Enable or disable EKF.
***bool:*** `True` | +| 🤖🖥️ | `use_sim` | Whether simulation is used.
***bool:*** `False` | +| 🤖🖥️ | `user_led_animations_file` | Path to a YAML file with a description of the user-defined animations.
***string:*** `''` | +| 🤖🖥️ | `wheel_config_path` | Path to wheel configuration file.
***string:*** [`{wheel_type}.yaml`](./panther_description/config) | +| 🤖🖥️ | `wheel_type` | Type of wheel. If you choose a value from the preset options ('WH01', 'WH02', 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path' parameters. For custom wheels, please define these parameters to point to files that accurately describe the custom wheels.
***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) | +| 🖥️ | `x` | Initial robot position in the global 'x' axis.
***float:*** `0.0` | +| 🖥️ | `y` | Initial robot position in the global 'y' axis.
***float:***` -2.0` | +| 🖥️ | `z` | Initial robot position in the global 'z' axis.
***float:*** `0.2` | +| 🖥️ | `roll` | Initial robot 'roll' orientation.
***float:*** `0.0` | +| 🖥️ | `pitch` | Initial robot 'pitch' orientation.
***float:*** `0.0` | +| 🖥️ | `yaw` | Initial robot 'yaw' orientation.
***float:*** `0.0` | + +> [!TIP] +> +> To read the arguments for individual packages, add the `-s` flag to the `ros2 launch` command (e.g. `ros2 launch panther_bringup bringup.launch.py ​​-s`) + ## Developer Info ### Setup pre-commit diff --git a/ROS_API.md b/ROS_API.md new file mode 100644 index 000000000..c7c91471e --- /dev/null +++ b/ROS_API.md @@ -0,0 +1,122 @@ +# ROS API + +> [!WARNING] +> **Beta Release** +> +> Please be advised that the software you are about to use is the Beta version of the ROS 2 Driver for Panther. It is functional and the architecture will not change significantly. It was tested by the Husarion team, however, some stability issues and bugs might still occur. Additionally, the ROS 2 API may face some minor changes in the following releases. +> +> We would be grateful for your feedback related to Panther ROS 2 driver. You can reach us the following ways: +> +> - by email at [support@husarion.com](mailto:support@husarion.com) +> - via our community forum: [Husarion Community](https://community.husarion.com) +> - using issue request on [GitHub](https://github.com/husarion/panther_ros/issues) + +## ROS 2 System Design + +This section describes the ROS packages in the Panther ROS system. These packages are located in the [panther_ros](https://github.com/husarion/panther_ros) GitHub repository. + +> [!NOTE] +> **Differences in ROS System** +> +> ROS 2 nodes differs slightly between **Panther v1.06** and **Panther v1.2+**. This is caused by internal hardware differences. Despite that, the ROS API was kept as closely matched between those revisions as possible and should be transparent in most of the use cases. + + + +The default way to communicate with Panther's hardware is via the Robot Operating System (ROS). All the drivers were written in ROS 2 framework. The ROS API is provided by ROS packages found in the GitHub repository [husarion/panther_ros](https://github.com/husarion/panther_ros). These packages are responsible for accessing the hardware components of the robot. + +The graph below represents Panther's ROS system. Some topics and services have been excluded from the graph for the sake of clarity. + +![Panther ROS 2 API Diagram](.docs/panther_ros2_api.drawio.svg) + +## ROS Interfaces + +Below is information about the physical robot API. For the simulation, topics and services are identical to the physical robot, but due to certain limitations, not all interfaces are present in the simulation. + +| Symbol | Meaning | +| ------ | ------------------------------- | +| 🤖 | Available for physical robot | +| 🖥️ | Available in simulation | +| ⚙️ | Requires specific configuration | + +### Nodes + +| | Node name | Description | +| --- | ------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🤖 | `battery_driver` | Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot.
[*panther_batter/battery_node*](./panther_batter) | +| 🤖🖥️ | `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)* | +| 🖥️ | `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) | +| 🤖🖥️ | `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/ControllerNode*](./panther_lights) | +| 🤖 | `lights_driver` | This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights.
[*panther_lights/DriverNode*](./panther_lights) | +| 🤖🖥️ | `lights_manager` | Node responsible for managing Bumper Lights animation scheduling.
[panther_manager/lights_manager](./panther_lights/) | +| 🤖🖥️⚙️ | `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)* | +| 🤖 | `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_status_node](./panther_diagnostics/)* | + +### 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/fix` | Raw GPS data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | +| 🤖🖥️⚙️ | `gps/filtered` | Filtered GPS position after fusing odometry data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | +| 🤖 | `hardware/e_stop` | Current E-stop state.
[*std_msgs/Bool*](https://docs.ros.org/en/latest/api/std_msgs/html/msg/Bool.html) | +| 🤖 | `hardware/io_state` | Current IO state.
[*panther_msgs/IOState*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `hardware/motor_controllers_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
[*panther_msgs/DriverState*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `imu/data` | Filtered IMU data.
[*sensor_msgs/Imu*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html) | +| 🤖🖥️ | `joint_states` | Provides information about the state of various joints in a robotic system.
[*sensor_msgs/JointState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html) | +| 🤖🖥️ | `lights/channel_1_frame` | Frame to be displayed on robot Front Bumper Lights.
[*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) | +| 🤖🖥️ | `lights/channel_2_frame` | Frame to be displayed on robot Rear Bumper Lights.
[*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) | +| 🤖🖥️ | `localization/set_pose` | Sets the pose of the EKF node.
[*geometry_msgs/PoseWithCovarianceStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseWithCovarianceStamped.html) | +| 🤖🖥️ | `odometry/filtered` | Contains information about the filtered position and orientation. When `localization_mode` is `relative`, the position and orientation are relative to the starting point. When `localization_mode` is `enu`, the orientation is relative to the east-north-up (ENU) coordinates.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | +| 🤖🖥️ | `odometry/wheels` | Robot odometry calculated from wheels.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | +| 🤖🖥️ | `robot_description` | Contains information about robot description from URDF file.
[*std_msgs/String*](https://docs.ros2.org/latest/api/std_msgs/msg/String.html) | +| 🤖 | `system_status` | State of the system, including Built-in Computer's CPU temperature and load.
[*panther_msgs/SystemStatus*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `tf` | Transforms of robot system.
[*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html) | +| 🤖🖥️ | `tf_static` | Static transforms of robot system.
[*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html) | + +#### Hidden topics + +| | Topic | Description | +| --- | ------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🤖 | `_battery/battery_1_status_raw` | First battery raw state.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | +| 🤖 | `_battery/battery_2_status_raw` | Second battery raw state. Published if second battery detected.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | +| 🤖🖥️⚙️ | `_odometry/gps` | Transformed raw GPS data to odometry format.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | + +### Services + +| | Service | Description | +| --- | ------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| 🤖🖥️ | `controller_manager/configure_controller` | Manage lifecycle transition.
[controller_manager_msgs/srv/ConfigureController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_controller_types` | Output the available controller types and their base classes.
[controller_manager_msgs/srv/ListControllerTypes](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_controllers` | Output the list of loaded controllers, their type and status.
[controller_manager_msgs/srv/ListControllers](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_hardware_components` | Output the list of available hardware components.
[controller_manager_msgs/srv/ListHardwareComponents](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_hardware_interfaces` | Output the list of available command and state interfaces.
[controller_manager_msgs/srv/ListHardwareInterfaces](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/load_controller` | Load a controller in a controller manager.
[controller_manager_msgs/srv/LoadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/reload_controller_libraries` | Reload controller libraries.
[controller_manager_msgs/srv/ReloadControllerLibraries](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/set_hardware_component_state` | Adjust the state of the hardware component.
[controller_manager_msgs/srv/SetHardwareComponentState](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/switch_controller` | Switch controllers in a controller manager.
[controller_manager_msgs/srv/SwitchController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/unload_controller` | Unload a controller in a controller manager.
[controller_manager_msgs/srv/UnloadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖 | `hardware/aux_power_enable` | Enables or disables AUX power.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/charger_enable` | Enables or disables external charger.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/digital_power_enable` | Enables or disables digital power.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/e_stop_reset` | Resets E-stop.
[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) | +| 🤖 | `hardware/e_stop_trigger` | Triggers E-stop.
[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) | +| 🤖 | `hardware/fan_enable` | Enables or disables fan.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/motor_power_enable` | Enables or disables motor power.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖🖥️ | `lights/set_animation` | Sets LED animation.
[panther_msgs/srv/SetLEDAnimation](https://github.com/husarion/panther_msgs) | +| 🤖 | `lights/set_brightness` | Sets global LED brightness, value ranges from **0.0** to **1.0**.
[panther_msgs/SetLEDBrightness](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `localization/enable` | Enable EKF node.
[std_srvs/srv/Empty](https://docs.ros2.org/latest/api/std_srvs/srv/Empty.html) | +| 🤖🖥️ | `localization/set_pose` | Set pose of EKF node.
[robot_localization/srv/SetPose](https://github.com/cra-ros-pkg/robot_localization/tree/ros2) | +| 🤖🖥️ | `localization/toggle` | Toggle filter processing in the EKF node.
[robot_localization/srv/ToggleFilterProcessing](https://github.com/cra-ros-pkg/robot_localization/tree/ros2) | diff --git a/panther_battery/README.md b/panther_battery/README.md index ed6bab6d0..2523e3b99 100644 --- a/panther_battery/README.md +++ b/panther_battery/README.md @@ -1,64 +1,39 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_battery -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - -Package containing nodes monitoring and publishing the internal battery state of the Husarion Panther robot. - -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) +The package containing nodes monitoring and publishing the internal battery state of the Husarion Panther robot. -## ROS Nodes +## Launch Files -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +This package contains: -### battery_driver +- `battery.launch.py`: Responsible for activating battery node, which dealing with reading and publishing battery data. -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) +## ROS Nodes -Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot. +### battery_node -[//]: # (ROS_API_NODE_DESCRIPTION_END) +Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier.versions of the robot. #### Publishes -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `_battery/battery_1_status_raw` [*sensor_msgs/BatteryState*]: first battery raw state. -- `_battery/battery_2_status_raw` [*sensor_msgs/BatteryState*]: second battery raw state. Published if second battery detected. -- `battery/battery_status` [*sensor_msgs/BatteryState*]: mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published. -- `battery/charging_status` [*panther_msgs/ChargingStatus*]: battery charging status. -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: battery diagnostic messages. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) +- `_battery/battery_1_status_raw` [*sensor_msgs/BatteryState*]: First battery raw state. +- `_battery/battery_2_status_raw` [*sensor_msgs/BatteryState*]: Second battery raw state. Published if second battery detected. +- `battery/battery_status` [*sensor_msgs/BatteryState*]: Mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published. +- `battery/charging_status` [*panther_msgs/ChargingStatus*]: Battery charging status. +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Battery diagnostic messages. #### Subscribes -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `hardware/io_state` [*panther_msgs/IOState*]: current state of IO. -- `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) +- `hardware/io_state` [*panther_msgs/IOState*]: Current state of IO. +- `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - - `~/adc/device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC nr 0 IIO device. Used with Panther version 1.2 and above. - `~/adc/device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC nr 1 IIO device. Used with Panther version 1.2 and above. -- `~/adc/ma_window_len/charge` [*int*, default: **10**]: window length of a moving average, used to smooth out battery charge readings. Used with Panther version 1.2 and above. -- `~/adc/ma_window_len/temp` [*int*, default: **10**]: window length of a moving average, used to smooth out battery temperature readings. Used with Panther version 1.2 and above. -- `~/battery_timeout` [*float*, default: **1.0**]: specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state. -- `~/ma_window_len/voltage` [*int*, default: **10**]: window length of a moving average, used to smooth out battery voltage readings. -- `~/ma_window_len/current` [*int*, default: **10**]: window length of a moving average, used to smooth out battery current readings. -- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: specifies timeout in seconds after which driver state messages will be considered old. Used with Panther version 1.06 and earlier. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) +- `~/adc/ma_window_len/charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings. Used with Panther version 1.2 and above. +- `~/adc/ma_window_len/temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings. Used with Panther version 1.2 and above. +- `~/battery_timeout` [*float*, default: **1.0**]: Specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state. +- `~/ma_window_len/voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings. +- `~/ma_window_len/current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings. +- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old. Used with Panther version 1.06 and earlier. diff --git a/panther_bringup/CMakeLists.txt b/panther_bringup/CMakeLists.txt index bd2e92f43..793057508 100644 --- a/panther_bringup/CMakeLists.txt +++ b/panther_bringup/CMakeLists.txt @@ -3,6 +3,6 @@ project(panther_bringup) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_bringup/README.md b/panther_bringup/README.md index dcc85e4ef..b12b4218d 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -1,162 +1,9 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_bringup -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) - -## Default Nodes Launched - -- `battery_driver` [*[panther_battery/battery_node](https://github.com/husarion/panther_ros/panther_battery/src/main.cpp)*]: node responsible for monitoring and publishing the internal Battery state of the Husarion Panther robot. For more information, refer to [panther_battery](https://github.com/husarion/panther_ros/panther_battery/README.md). -- `ekf_filter` [*[robot_localization/ekf_node](https://github.com/cra-ros-pkg/robot_localization/blob/humble-devel/src/ekf_node.cpp)*]: Extended Kalman Filter node for more accurate odometry. For more information, refer to [robot_localization](https://github.com/cra-ros-pkg/robot_localization/tree/noetic-devel). The default configuration is stored in [ekf_config.yaml](https://github.com/husarion/panther_ros/panther_gazebo/config/ekf_config.yaml). -- `imu_container` [*[phidgets_spatial/phidgets::SpatialRosI](https://github.com/ros-drivers/phidgets_drivers/blob/humble/phidgets_spatial/src/spatial_ros_i.cpp)*, *[imu_filter_madgwick/ImuFilterMadgwickRos](https://github.com/CCNYRoboticsLab/imu_tools/blob/humble/imu_filter_madgwick/src/imu_filter_node.cpp)*]: container responsible for running Phidget Spatial IMU ROS driver, filtering and fusing the IMU data. It composes the `phidgets_spatial_node` and `imu_filter_node`. - -## Bringup Launch Arguments - -- `battery_config_path` [*string*, default: **None**]: path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. -- `controller_config_path` [*string*, default: **panther_controller/config/_controller.yaml**]: path to controller configuration file. A path to custom configuration can be specified here. -- `disable_manager` [*bool*, default: **false**]: Enable or disable manager_bt_node. -- `ekf_config_path` [*string*, default: **panther_bringup/config/ekf.yaml**]: path to the EKF configuration file. -- `led_config_file` [*string*, default: **panther_lights/config/led_config.yaml**]: path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. -- `namespace` [*string*, default: **None**] Add namespace to all launched nodes. -- `publish_robot_state` [*bool*, default: **true**]: whether to publish the default Panther robot description. -- `shutdown_hosts_config_path` = [*string*, default: **panther_bringup/config/shutdown_hosts.yaml**]: Path to file with list of hosts to request shutdown. -- `use_ekf` [*bool*, default: **true**]: enable or disable Extended Kalman Filter. -- `use_sim` [*bool*, default: **false**]: whether simulation is used. -- `user_led_animations_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. -- `wheel_config_path` [*string*, default: **$(find panther_description)/config/.yaml**]: path to YAML file with wheel specification. Arguments become required if `wheel_type` is set to **custom**. -- `wheel_type` [*string*, default: **WH01**]: type of wheel, possible are: **WH01** - off-road, **WH02** - mecanum, **WH04** - small pneumatic, and **custom** - custom wheel types (requires setting `wheel_config_path` argument accordingly). -- `components_config_path` [*string*, default: **None**]: Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). Example of configuration you can find [config/components.yaml](config/components.yaml) - -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - -## External ROS Nodes - -[//]: # (ROS_API_PACKAGE_NAME_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### ekf_filter - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type: *[robot_localization/ekf_node](https://github.com/cra-ros-pkg/robot_localization/blob/humble-devel/src/ekf_node.cpp)*. - -Extended Kalman Filter node for more accurate odometry. For more information, refer to [robot_localization](https://github.com/cra-ros-pkg/robot_localization/tree/humble-devel). The default configuration is stored in `panther_bringup/config/ekf.yaml`. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Subscribers - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `~/odometry/wheels` [*nav_msgs/Odometry*]: robot odometry calculated from wheels. -- `~/imu/data` [*sensor_msgs/Imu*]: filtered IMU data. -- `/tf` [*tf2_msgs/TFMessage*]: transforms of robot system. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `~/odometry/filtered` [*nav_msgs/Odometry*]: provides filtered odometry information. This topic contains a fused and enhanced estimate of the robot's pose and velocity, incorporating data from various sensors and correcting for any errors in the estimated state. -- `/tf` [*tf2_msgs/TFMessage*]: publishes `odom` to `base_link` transform. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) - -#### Service Servers - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) - -- `localization/set_pose` [*robot_localization/SetPose*]: by issuing a *geometry_msgs/PoseWithCovarianceStamped* message to the set_pose topic, users can manually set the state of the filter. This is useful for resetting the filter during testing and allows for interaction with RViz. Alternatively, the state estimation nodes advertise a SetPose service, whose type is *robot_localization/SetPose*. - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### imu_filter_node - -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type: *[imu_filter_madgwick/imu_filter_node](https://github.com/CCNYRoboticsLab/imu_tools/blob/humble/imu_filter_madgwick/src/imu_filter_node.cpp)*. - -Node responsible for filtering and fusing raw data from the IMU. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Subscribers - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `~/imu/data_raw` [*sensor_msgs/Imu*]: the raw accelerometer and gyroscope data. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `~/imu/data` [*sensor_msgs/Imu*]: the fused IMU messages, containing the orientation. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### phidgets_spatial_node - -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type: *[phidgets_spatial/spatial_ros_i.cpp](https://github.com/ros-drivers/phidgets_drivers/blob/humble/phidgets_spatial/src/spatial_ros_i.cpp)*. - -The ROS driver for Phidgets Spatial. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `~/imu/data_raw` [*sensor_msgs/Imu*]: the raw accelerometer and gyroscope data. -- `~/imu/is_calibrated` [*std_msgs/Bool*]: whether the gyroscope has been calibrated. This will be done automatically at startup time but can also be re-done at any time by calling the `imu/calibrate` service. -- `~/imu/mag` [*sensor_msgs/MagneticField*]: the raw magnetometer data. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) - -#### Service Servers - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) - -- `~/imu/calibrate` [*std_srvs/Empty*]: run calibration on the gyroscope. +## Launch Files -[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) -[//]: # (ROS_API_NODE_END) +This package contains: -[//]: # (ROS_API_PACKAGE_END) +- `bringup.launch.py`: Responsible for activating whole robot system. diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index 8e1b3c12b..20fd2664b 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): "disable_manager", default_value="False", description="Enable or disable manager_bt_node.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) namespace = LaunchConfiguration("namespace") @@ -48,7 +48,7 @@ def generate_launch_description(): "use_ekf", default_value="True", description="Enable or disable EKF.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) serial_no = EnvironmentVariable(name="PANTHER_SERIAL_NO", default_value="----") diff --git a/panther_controller/CONFIGURATION.md b/panther_controller/CONFIGURATION.md new file mode 100644 index 000000000..f541b8727 --- /dev/null +++ b/panther_controller/CONFIGURATION.md @@ -0,0 +1,9 @@ +# panther_controller + +## Changing Velocity Smoothing Parameters + +The Panther by default uses [diff_drive_controller](https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html) from [ros2 control](https://control.ros.org/master/index.html) or [mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller). This controller can be customized, among others: by modifying the robot's dynamic parameters (e.g. smooth the speed or limit the robot's speed and acceleration). Its parameters can be found in the [panther_controller](https://github.com/husarion/panther_ros/tree/ros2-devel/panther_controller/config). By default, these values correspond to the upper limits of the robot's velocities and accelerations. + +## Changing Wheel Type + +Changing wheel types is possible and can be done for both the real robot and the simulation. By default, three types of wheels are supported using the launch argument `wheel_type`. If you want to use custom wheels, all you need to do is point to the new wheel and controller configuration files using the `wheel_config_path` and `controller_config_path` parameters. These files should be based on the default files, i.e. [WH01_controller.yaml](./config/WH01_controller.yaml) and [WH01.yaml](../panther_description/config/WH01.yaml). diff --git a/panther_controller/README.md b/panther_controller/README.md index dc373312e..ac600dc72 100644 --- a/panther_controller/README.md +++ b/panther_controller/README.md @@ -1,122 +1,13 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_controller -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) - -## Default Nodes Launched - -- `ros2_control_node` [*[controller_manager/controller_manager](https://github.com/ros-controls/ros2_control/blob/master/controller_manager)*]: Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. For more information, refer to [controller_manager](https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html). This node manages the following controllers: - - `imu_broadcaster` [*[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)*]: The broadcaster to publish readings of IMU sensors. - - `joint_state_broadcaster` [*[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)*]: The broadcaster reads all state interfaces and reports them on specific topics. - - `drive_controller` [*[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller)* ]: Controller which manages mobile robots with a differential drive. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. -- `robot_state_publisher` [*[robot_state_publisher/robot_state_publisher](https://github.com/ros/robot_state_publisher)*]: The Robot State Publisher broadcasts a robot's state to tf2 using a provided URDF model and joint states. It updates the model and broadcasts poses for fixed and movable joints to tf2 topics. - -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - -## External ROS Nodes - -[//]: # (ROS_API_PACKAGE_NAME_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### imu_broadcaster - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type:*[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)*. - -The broadcaster to publish readings of IMU sensors. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `imu/data` [*sensor_msgs/msg/Imu*]: data from IMU sensor. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### joint_state_broadcaster - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type:*[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)*. - -The broadcaster reads all state interfaces and reports them on specific topics. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `dynamic_joint_states` [*control_msgs/msg/DynamicJointState*] - provides information about the state of various movable joints in a robotic system. -- `joint_states` [*sensor_msgs/msg/JointState*] - provides information about the state of various joints in a robotic system. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### drive_controller - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type:*[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller)*. - -Controller which manages mobile robots with a differential drive. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Subscribers - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `cmd_vel` [*geometry_msgs/msg/Twist*]: command linear and angular velocity values. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) +## Launch Files -- `/tf` [*tf2_msgs/msg/TFMessage*]: tf tree. Published only if `enable_odom_tf=true` -- `odometry/wheels` [*nav_msgs/msg/Odometry*]: odometry data from wheel encoders. +- `controller.launch.py`: Establishes communication with the hardware by loading the robot's URDF with plugins and configures the controllers to exchange information between the engine driver and the IMU. -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) +## Configuration Files -[//]: # (ROS_API_PACKAGE_END) +- [`WH01_controller.yaml`](./config/WH01_controller.yaml): Configures `imu_broadcaster`, `joint_state_broadcaster` and `drive_controller` controllers for default WH01 wheels. +- [`WH02_controller.yaml`](./config/WH02_controller.yaml): Configures `imu_broadcaster`, `joint_state_broadcaster` and `drive_controller` controllers for mecanum WH02 wheels. +- [`WH04_controller.yaml`](./config/WH04_controller.yaml): Configures `imu_broadcaster`, `joint_state_broadcaster` and `drive_controller` controllers for small pneumatic WH04 wheels. diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py index 8a34ef7a7..92c9b7a63 100644 --- a/panther_controller/launch/controller.launch.py +++ b/panther_controller/launch/controller.launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): ), description=( "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/_controller.yaml'. You can also specify" + " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify" " the path to your custom controller configuration file here. " ), ) @@ -93,7 +93,7 @@ def generate_launch_description(): "Whether to launch the robot_state_publisher node." "When set to False, users should publish their own robot description." ), - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) wheel_config_path = LaunchConfiguration("wheel_config_path") @@ -101,7 +101,7 @@ def generate_launch_description(): "use_sim", default_value="False", description="Whether simulation is used", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) declare_wheel_config_path_arg = DeclareLaunchArgument( @@ -115,7 +115,7 @@ def generate_launch_description(): ), description=( "Path to wheel configuration file. By default, it is located in " - "'panther_description/config/.yaml'. You can also specify the path " + "'panther_description/config/{wheel_type}.yaml'. You can also specify the path " "to your custom wheel configuration file here. " ), ) diff --git a/panther_description/README.md b/panther_description/README.md index 81a4197bc..6c15c4c40 100644 --- a/panther_description/README.md +++ b/panther_description/README.md @@ -1,3 +1,14 @@ # panther_description The package contains URDF files responsible for creating a representation of the robot by specifying the relationships and types of connections (joints) between individual links. It also contains information about the robot's mesh. + +## Launch Files + +- `load_urdf.launch.py` - loads the robot's URDF and creates simple bindings to display moving joints. + +## Configuration Files + +- [`components.yaml`](./config/components.yaml): Allows you to quickly add visualization of sensors, TF connections and simulate their behavior in the simulator. +- [`WH01.yaml`](./config/WH01.yaml): Description of physical and visual parameters for the wheel WH01. +- [`WH02.yaml`](./config/WH02.yaml): Description of physical and visual parameters for the wheel WH02. +- [`WH04.yaml`](./config/WH04.yaml): Description of physical and visual parameters for the wheel WH04. diff --git a/panther_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py index d8b89e84c..13a0b83d9 100644 --- a/panther_description/launch/load_urdf.launch.py +++ b/panther_description/launch/load_urdf.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): "add_wheel_joints", default_value="True", description="Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) battery_config_path = LaunchConfiguration("battery_config_path") @@ -78,7 +78,7 @@ def generate_launch_description(): ), description=( "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/_controller.yaml'. You can also specify" + " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify" " the path to your custom controller configuration file here. " ), ) @@ -95,7 +95,7 @@ def generate_launch_description(): "use_sim", default_value="False", description="Whether simulation is used.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) declare_wheel_config_path_arg = DeclareLaunchArgument( @@ -109,7 +109,7 @@ def generate_launch_description(): ), description=( "Path to wheel configuration file. By default, it is located in " - "'panther_description/config/.yaml'. You can also specify the path " + "'panther_description/config/{wheel_type}.yaml'. You can also specify the path " "to your custom wheel configuration file here. " ), ) diff --git a/panther_diagnostics/README.md b/panther_diagnostics/README.md index 0c85ba3ec..e4d412464 100644 --- a/panther_diagnostics/README.md +++ b/panther_diagnostics/README.md @@ -1,50 +1,31 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_diagnostics -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - Package containing nodes monitoring and publishing the Built-in Computer status of Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) +## Launch Files -## ROS Nodes +- `system_status.launch.py`: Launch a node that analyzes the state of the most important components in the robot -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +## Configuration Files -### system_monitor +- [`system_status_parameters.yaml`](./config/system_status_parameters.yaml): Defines parameters for `system_status_node`. -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) +## ROS Nodes -Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and CPU temperature. +- [`system_status_node`](#system_status_node): Publishes system state of the Built-in Computer such as CPU usage, RAM usage, disk usage and CPU temperature. -[//]: # (ROS_API_NODE_DESCRIPTION_END) +### system_status_node #### Publishes -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: system status diagnostic messages. -- `system_status` [*panther_msgs/SystemStatus*]: system status statistics. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System status diagnostic messages. +- `system_status` [*panther_msgs/SystemStatus*]: State of the system, including Built-in Computer's CPU temperature and load. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - -- `~frame_id` [*string*, default: **build_in_computer**]: Frame where computer is located. +- `~frame_id` [*string*, default: **built_in_computer**]: Frame where computer is located. - `~publish_rate` [*double*, default: **0.25**]: System status publish rate in seconds. - `~disk_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for disk usage warning in percentage. - `~memory_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for memory usage warning in percentage. - `~cpu_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for CPU usage warning in percentage. - `~cpu_temperature_warn_threshold` [*float*, default: **80.0**]: Threshold for CPU temperature warning in degrees Celsius. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) diff --git a/panther_gazebo/CONFIGURATION.md b/panther_gazebo/CONFIGURATION.md new file mode 100644 index 000000000..8a8c4c198 --- /dev/null +++ b/panther_gazebo/CONFIGURATION.md @@ -0,0 +1,61 @@ +# panther_gazebo + +## Use of GPS in Simulation + +The NavSat system is utilized to publish the Panther robot's position within the Gazebo world. It manages navigation satellite sensors, such as GPS, which report position and velocity in spherical coordinates (latitude/longitude) through Ignition Transport. + +The NavSat sensors requires the spherical coordinates of the world origin to be configured. This configuration can be accomplished, for instance, by employing the `` tag within the world's SDF or by utilizing the Ignition `/world/world_name/set_spherical_coordinates` service. + +To obtain GPS data in Ignition, follow these steps: + +- Include the [ANT02](https://github.com/husarion/ros_components_description/blob/ros2/urdf/external_antenna.urdf.xacro) by adding the following lines to your [components.yaml](../panther_description/config/components.yaml) file inside the `components` list: + +```yaml + - type: ANT02 + parent_link: cover_link + xyz: 0.185 -0.12 0.0 + rpy: 0.0 0.0 3.14 + device_namespace: gps +``` + +- Add the following tag to your world's SDF file and specify this file using the `world` parameter (the default `husarion_world.sdf` file already includes this tag): + +```xml + + EARTH_WGS84 + ENU + 53.1978 + 18.3732 + 0 + 0 + +``` + +## Linear Battery Plugin + +It is possible to simulate the battery operation of the Panther robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin_config.yaml` file or in the file pointed to by the `battery_config_path` parameter. Below, you will find the plugin parameters that enable battery simulation. + +- `simulate_discharging` [*bool*, default: **false**]: Enables battery simulation. If set to `true`, the battery will discharge **at a constant rate** (regardless of joint torque), and if it depletes completely, the robot will stop moving. When set to `false`, the battery will not discharge, but the battery status information will still be published on the `battery/battery_status` topic. +- `initial_charge_percentage` [*float*, default: **70.0**]: Sets the initial charge percentage of the battery. +- `charging_time` [*float*, default: **6.0**]: Specifies the charging time for the battery in hours. +- `power_load` [*float*, default: **120.0**]: Represents the average power load during normal operation **[W]** and is initially set to 120.0 W. With the default power load of 120.0 W, the robot can operate for up to 8 hours. When the `simulate_discharging` parameter is set to `false`, this value defaults to 0.0 W. Users are encouraged to customize this value to match their specific requirements. For more information on Panther power consumption, please refer to [Panther Battery & Charging Documentation](https://husarion.com/manuals/panther/#battery--charging). + +> [!NOTE] +> +> The battery model is quite simple and involves significant simplifications. As a result, the battery discharge rate observed on the physical robot may differ from the model's predictions. + +### Charging Process + +Unfortunately, there is no straightforward way to exchange `LinearBatteryPlugin` services between ROS and Gazebo Transport, so you need to use the `ign` commands. As a result, the method of charging differs between the real and simulated robot. + +You can start the charging process by calling the Ignition service: + +```bash +ign service --service /model/panther/battery/battery/recharge/start --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 +``` + +and stop it using: + +```bash +ign service --service /model/panther/battery/battery/recharge/stop --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 +``` diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md index 1c7783994..662f6ac49 100644 --- a/panther_gazebo/README.md +++ b/panther_gazebo/README.md @@ -2,97 +2,15 @@ The package contains a launch file and source files used to run the robot simulation in Gazebo. The simulator tries to reproduce the behavior of a real robot as much as possible, including the provision of an analogous ROS_API. -## Available Launch File +## Launch Files -- `spawn_robot.launch.py` - is responsible for spawning the robot in the simulator -- `simulate_robot.launch.py` - is responsible for giving birth to the robot and simulating its physical behavior, such as driving, displaying data, etc. -- `simulate_multiple_robots.launch.py` - similar to the above with logic allowing you to quickly add a swarm of robots -- **`simulation.launch.py`** - a target file that runs the gazebo simulator and adds and simulates the robot's behavior in accordance with the given arguments. +- `spawn_robot.launch.py`: Responsible for spawning the robot in the simulator. +- `simulate_robot.launch.py`: Responsible for giving birth to the robot and simulating its physical behavior, such as driving, displaying data, etc. +- `simulate_multiple_robots.launch.py`: Similar to the above with logic allowing you to quickly add a swarm of robots. +- **`simulation.launch.py`**: A target file that runs the gazebo simulator that adds and simulates the robot's behavior in accordance with the given arguments. -## Usage +## Configuration Files -The recommended method for launching the simulation is by utilizing the [simulation.launch.py](https://github.com/husarion/panther_ros/panther_gazebo/launch/simulation.launch.py) file. Below, you will find launch arguments that enable simulation configuration. You can also launch more robots using `spawn.launch.py` ​​after the system has been started. - -### Launch Arguments - -- `add_world_transform` [*bool*, default: **False**]: Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots). -- `battery_config_path` [*string*, default: **panther_gazebo/config/battery_plugin_config.yaml**]: Path to the Ignition `LinearBatteryPlugin` configuration file. This configuration is intended for use in simulations only. For more information on how to configure this plugin, please refer to the [Linear Battery Plugin](#linear-battery-plugin) section. -- `controller_config_path` [*string*, default: **panther_controller/config/_controller.yaml**]: Path to the controller configuration file. If you want to use a custom configuration, you can specify the path to your custom controller configuration file here. -- `gz_bridge_config_path` [*string*, default: **panther_gazebo/config/gz_bridge.yaml**]: Path to the `parameter_bridge` configuration file. For detailed information on configuring the `parameter_bridge`, please refer to this [example](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge#example-5-configuring-the-bridge-via-yaml). -- `gz_log_level` [*[0-4]*, default: **1**]: Adjust the level of console output. -- `headless_mode` [*bool*, default: **False**]: Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the amount of calculations. -- `x` [*float*, default: **5.0**]: spawn position **[m]** of the robot in the world in **X** direction. -- `y` [*float*, default: **-5.0**]: spawn position **[m]** of the robot in the world in **Y** direction. -- `z` [*float*, default: **0.2**]: spawn position **[m]** of the robot in the world in **Z** direction. -- `roll` [*float*, default: **0.0**]: spawn roll angle **[rad]** of the robot in the world. -- `pitch` [*float*, default: **0.0**]: spawn pitch angle **[rad]** of the robot in the world. -- `yaw` [*float*, default: **0.0**]: spawn yaw angle **[rad]** of the robot in the world. -- `publish_robot_state` [*bool*, default: **true**]: Whether to launch the robot_state_publisher node. When set to `false`, users should publish their own robot description. -- `robots` [*yaml style*, default: **""**]: The list of the robots spawned in the simulation e.g. robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}'" -- `wheel_config_path` [*string*, default: **panther_description/config/.yaml**]: Path to the wheel configuration file. If you want to use a custom configuration, you can specify the path to your custom wheel configuration file here. Please refer to the `wheel_type` parameter description for more information. -- `wheel_type` [*string*, default: **WH01**]: Specify the type of wheel. If you select a value from the provided options (`WH01`, `WH02`, `WH04`), you can disregard the `wheel_config_path` and `controller_config_path` parameters. If you have custom wheels, set this parameter to `CUSTOM` and provide the necessary configurations. -- `world` [*string*, default: **-r /worlds/husarion_world.sdf**]: path to Gazebo world file used for simulation. - -### Changing Wheel Type - -It is possible to change Panther wheels model in simulation. All you need to do is to point to new wheel and controller configuration files using `wheel_config_path` and `controller_config_path` parameters. These files should be based on the default ones, i.e., [WH01_controller.yaml](https://github.com/husarion/panther_ros/panther_controller/config/WH01_controller.yaml) and [WH01.yaml](https://github.com/husarion/panther_ros/panther_description/config/WH01.yaml). - -### Linear Battery Plugin - -It is possible to simulate the battery operation of the Panther robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin_config.yaml` file or in the file pointed to by the `battery_config_path` parameter. Below, you will find the plugin parameters that enable battery simulation. - -- `simulate_discharging` [*bool*, default: **false**]: Enables battery simulation. If set to `true`, the battery will discharge **at a constant rate** (regardless of joint torque), and if it depletes completely, the robot will stop moving. When set to `false`, the battery will not discharge, but the battery status information will still be published on the `battery/battery_status` topic. -- `initial_charge_percentage` [*float*, default: **70.0**]: Sets the initial charge percentage of the battery. -- `charging_time` [*float*, default: **6.0**]: Specifies the charging time for the battery in hours. -- `power_load` [*float*, default: **120.0**]: Represents the average power load during normal operation **[W]** and is initially set to 120.0 W. With the default power load of 120.0 W, the robot can operate for up to 8 hours. When the `simulate_discharging` parameter is set to `false`, this value defaults to 0.0 W. Users are encouraged to customize this value to match their specific requirements. For more information on Panther power consumption, please refer to [Panther Battery & Charging Documentation](https://husarion.com/manuals/panther/#battery--charging). - -> **Note** -> -> The `percentage` field has a range of 0.0-100.0. This differs from the real functioning of Panther, where, in accordance with the BatteryState message definition, the value is within the range of 0.0-1.0. - -#### Charging Process - -Unfortunately, there is no straightforward way to exchange `LinearBatteryPlugin` services between ROS and Gazebo Transport, so you need to use the `ign` commands. As a result, the method of charging differs between the real and simulated robot. - -You can start the charging process by calling the Ignition service: - -```bash -ign service --service /model/panther/battery/panther_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/panther_battery/recharge/stop --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 -``` - -### Use of GPS in Simulation - -The NavSat system is utilized to publish the Panther robot's position within the Gazebo world. It manages navigation satellite sensors, such as GPS, which report position and velocity in spherical coordinates (latitude/longitude) through Ignition Transport. - -The NavSat sensors requires the spherical coordinates of the world origin to be configured. This configuration can be accomplished, for instance, by employing the `` tag within the world's SDF or by utilizing the Ignition `/world/world_name/set_spherical_coordinates` service. - -To obtain GPS data in Ignition, follow these steps: - -- Include the [ANT02](https://github.com/husarion/ros_components_description/blob/ros2/urdf/external_antenna.urdf.xacro) by adding the following lines to your [components.yaml](https://github.com/husarion/panther_ros/blob/ros2/panther_description/config/components.yaml) file inside the `components` list: - -```yaml - - type: ANT02 - parent_link: cover_link - xyz: 0.185 -0.12 0.0 - rpy: 0.0 0.0 3.14 - device_namespace: gps -``` - -- Add the following tag to your world's SDF file and specify this file using the `world` parameter (the default `husarion_world.sdf` file already includes this tag): - -```xml - - EARTH_WGS84 - ENU - 53.1978 - 18.3732 - 0 - 0 - -``` +- [`battery_plugin_config.yaml`](./config/battery_plugin_config.yaml): Simulated LinearBatteryPlugin configuration. +- [`gz_bridge.yaml`](./config/gz_bridge.yaml): Specify data to exchange between ROS and Gazebo simulation. +- [`led_strips.yaml`](./config/led_strips.yaml): Configure properties of led strips in simulation to animate lights. diff --git a/panther_gazebo/launch/simulate_multiple_robots.launch.py b/panther_gazebo/launch/simulate_multiple_robots.launch.py index 4720cdc48..9a9dc497a 100644 --- a/panther_gazebo/launch/simulate_multiple_robots.launch.py +++ b/panther_gazebo/launch/simulate_multiple_robots.launch.py @@ -37,6 +37,7 @@ def generate_launch_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( diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index ef9f5640e..512b3a452 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -77,7 +77,7 @@ def generate_launch_description(): "use_ekf", default_value="True", description="Enable or disable EKF.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) spawn_robot_launch = IncludeLaunchDescription( diff --git a/panther_hardware_interfaces/README.md b/panther_hardware_interfaces/README.md index e42fcc20e..8ad8af4f1 100644 --- a/panther_hardware_interfaces/README.md +++ b/panther_hardware_interfaces/README.md @@ -1,48 +1,24 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_hardware_interfaces -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - Package that implements SystemInterface from ros2_control for Panther. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) - ## ROS Nodes -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +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/)). ### PantherSystem -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -This package doesn't contain any standalone nodes - `PantherSystem` is a plugin loaded by the resource manager. -To use this hardware interface you have to add it to your URDF (you can check how to do it in [panther_description](https://github.com/husarion/panther_ros/panther_description/)) and add a controller (example configuration provided in [panther_controller](https://github.com/husarion/panther_ros/panther_controller/) package). -That said apart from the usual interface provided by the ros2_control, this plugin also provides additional published topics and services specific for Panther. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) +Plugin responsible for communicating with engine controllers via the CAN bus, providing E-Stop functionalities and managing Built-in Computer GPIO ports. #### Publishers -[//]: # (ROS_API_NODE_PUBLISHERS_START) - - `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther 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. -[//]: # (ROS_API_NODE_PUBLISHERS_END) - #### Service Servers -[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) - - `hardware/aux_power_enable` [*std_srvs/SetBool*]: Enables or disables AUX power. - `hardware/charger_enable` [*std_srvs/SetBool*]: Enables or disables charger. - `hardware/digital_power_enable` [*std_srvs/SetBool*]: Enables or disables digital power. @@ -52,61 +28,44 @@ That said apart from the usual interface provided by the ros2_control, this plug - `hardware/led_control_enable` [*std_srvs/SetBool*]: Enables or disables SBC (Single Board Computer) control over the LEDs. - `hardware/motor_power_enable` [*std_srvs/SetBool*]: Enables or disables motor power. -[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) - #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - -Parameters that are required, are defined when including interface in URDF (you can check out [panther_macro.urdf.xacro](https://github.com/husarion/panther_ros/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)). Physical properties -- `encoder_resolution` [*int*, default: **1600**]: property of the encoder used, shouldn't be changed. -- `gear_ratio` [*float*, default: **30.08**]: property of the gearbox used, shouldn't be changed. -- `motor_torque_constant` [*float*, default: **0.11**]: same as set in the Roboteq driver (TNM parameter), also shouldn't be changed, as it is measured property of the motor. -- `max_rpm_motor_speed` [*float*, default: **3600.0**]: max RPM speed set in the Roboteq driver (MXRPM parameter). -- `gearbox_efficiency` [*float*, default: **0.75**]: measured efficiency, used for converting read current to torque, can vary depending on different factors such as temperature and wear. +- `encoder_resolution` [*int*, default: **1600**]: Property of the encoder used, shouldn't be changed. +- `gear_ratio` [*float*, default: **30.08**]: Property of the gearbox used, shouldn't be changed. +- `motor_torque_constant` [*float*, default: **0.11**]: Same as set in the Roboteq driver (TNM parameter), also shouldn't be changed, as it is measured property of the motor. +- `max_rpm_motor_speed` [*float*, default: **3600.0**]: Max RPM speed set in the Roboteq driver (MXRPM parameter). +- `gearbox_efficiency` [*float*, default: **0.75**]: Measured efficiency, used for converting read current to torque, can vary depending on different factors such as temperature and wear. CAN settings -- `can_interface_name` [*string*, default: **panther_can**]: name of the CAN interface. -- `master_can_id` [*int*, default: **3**]: CAN ID of the master device (set as in [canopen_configuration.yaml](https://github.com/husarion/panther_ros/panther_hardware_interfaces/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](https://github.com/husarion/panther_ros/panther_hardware_interfaces/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](https://github.com/husarion/panther_ros/panther_hardware_interfaces/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. -- `driver_states_update_frequency` [*float*, default: **20.0**]: as by default, the driver state is published with lower frequency, it also shouldn't be updated with every controller loop iteration. The exact frequency at which driver state is published won't match this value - it will also depend on the frequency of the controller (the exact value of the period can be calculated with the following formula `controller_frequency / ceil(controller_frequency / driver_states_update_frequency)`). -- `max_roboteq_initialization_attempts` [*int*, default: **5**]: in some cases, an SDO error can happen during initialization, it is possible to configure more attempts, before escalating to a general error. -- `max_roboteq_activation_attempts` [*int*, default: **5**]: similar to initialization, it is possible to allow some SDO errors before escalating to error. -- `max_write_pdo_cmds_errors_count` [*int*, default: **2**]: how many consecutive errors can happen before escalating to general error. -- `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. +- `can_interface_name` [*string*, default: **panther_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. +- `driver_states_update_frequency` [*float*, default: **20.0**]: As by default, the driver state is published with lower frequency, it also shouldn't be updated with every controller loop iteration. The exact frequency at which driver state is published won't match this value - it will also depend on the frequency of the controller (the exact value of the period can be calculated with the following formula `controller_frequency / ceil(controller_frequency / driver_states_update_frequency)`). +- `max_roboteq_initialization_attempts` [*int*, default: **5**]: In some cases, an SDO error can happen during initialization, it is possible to configure more attempts, before escalating to a general error. +- `max_roboteq_activation_attempts` [*int*, default: **5**]: Similar to initialization, it is possible to allow some SDO errors before escalating to error. +- `max_write_pdo_cmds_errors_count` [*int*, default: **2**]: How many consecutive errors can happen before escalating to general error. +- `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. > [!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. -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_NAME_START) - ### PantherImuSensor -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -This package doesn't contain any standalone nodes - `PantherImuSensor` is a plugin loaded by the resource manager. -To use this hardware interface you have to add it to your URDF (you can check how to do it in [panther_description](https://github.com/husarion/panther_ros/tree/ros2-devel/panther_description/)) and add an `imu_sensor_broadcaster` controller (example configuration provided in [panther_controller](https://github.com/husarion/panther_ros/tree/ros2-devel/panther_controller/) package). - -[//]: # (ROS_API_NODE_DESCRIPTION_END) +Plugin responsible for communicating with IMU and filtering data using Madgwick Filter. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - -Parameters that are required, are defined when including interface in URDF (you can check out [panther_macro.urdf.xacro](https://github.com/husarion/panther_ros/tree/ros2-devel/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)). Physical properties @@ -165,12 +124,9 @@ gain = sqrt(3/4)* gyroMeasError = 0.00303 zeta = sqrt(3/4)* gyroMeasDrift = 0.00151 ``` -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) - ## Code structure -The code structure is described in more detail in a [separate file](https://github.com/husarion/panther_ros/panther_hardware_interfaces/CODE_STRUCTURE.md). +The code structure is described in more detail in a [separate file](./CODE_STRUCTURE.md). ## Generating CAN config diff --git a/panther_lights/CONFIGURATION.md b/panther_lights/CONFIGURATION.md new file mode 100644 index 000000000..75f1d4345 --- /dev/null +++ b/panther_lights/CONFIGURATION.md @@ -0,0 +1,167 @@ +# panther_light + +## 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: + +| ID | NAME | PRIORITY | ANIMATION | +| :---: | ----------------- | :------: | -------------------------------------------------- | +| 0 | E_STOP | 3 | ![E_STOP](.docs/E_STOP.webp) | +| 1 | READY | 3 | ![READY](.docs/READY.webp) | +| 2 | ERROR | 1 | ![ERROR](.docs/ERROR.webp) | +| 3 | MANUAL_ACTION | 3 | ![MANUAL_ACTION](.docs/MANUAL_ACTION.webp) | +| 4 | AUTONOMOUS_ACTION | 3 | ![AUTONOMOUS_ACTION](.docs/AUTONOMOUS_ACTION.webp) | +| 5 | GOAL_ACHIEVED | 2 | ![GOAL_ACHIEVED](.docs/GOAL_ACHIEVED.webp) | +| 6 | LOW_BATTERY | 2 | ![LOW_BATTERY](.docs/LOW_BATTERY.webp) | +| 7 | CRITICAL_BATTERY | 2 | ![CRITICAL_BATTERY](.docs/CRITICAL_BATTERY.webp) | +| 9 | CHARGING_BATTERY | 3 | ![CHARGING_BATTERY](.docs/CHARGING_BATTERY.webp) | + +### Panels + +The `panels` section of the YAML file lists all the physical LED panels on the robot. Each panel has two attributes: + +- `channel` [*int*, default: **None**] the identifier for the LED panel. It is used to differentiate between multiple panels. +- `number_of_leds`: defines the total number of LEDs present on the panel. + +### Segments + +The `segments` section is used to create virtual segments on the robot by dividing the LED panels into different parts. This allows for more precise control over which LEDs are lit up for different effects or indicators. Each segment has three attributes: + +- `name`: the identifier for the segment, such as "front" or "rear". It is used to differentiate between multiple segments. +- `channel`: This specifies which LED panel the segment belongs to. It has to match one of the channels defined in the `panels` section. +- `led_range`: This defines the range of LEDs within the panel that the segment covers. The range is specified as a start-end pair (e.g. 0-45). The range can be specified in reverse order (e.g. 45-0), which may be useful for wiring or orientation reasons. + +### Segments map + +The `segments_map` section allows creating named groups of segments on which animations can be displayed. Each entry under `segments_map` consists of a key representing the group name and a list of segments included in the group. Segment names have to match one of the segments defined in the `segments` section. By default, you can use provided mapping: + +- `all` [*list*, default: **None**]: Grouping both `front` and `rear` segments together. +- `front` [*list*, default: **None**]: Containing only the `front` segment. +- `rear` [*list*, default: **None**]: Containing only the `rear` segment. + +### Animations + +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`. + - `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. +- `name` [*string*, default: **ANIMATION_`ID`**]: name of an animation. If not provided, it will default to **ANIMATION_`ID`**, where `ID` is equal to `id` parameter of the given animation. +- `priority` [*int*, default: **3**]: priority at which animation will be placed in the queue. The list below shows the behavior when an animation with a given ID arrives: + - **1** interrupts and removes animation with priorities **2** and **3**. + - **2** interrupts animations with priority **3**. + - **3** adds animation to the end of the queue. +- `timeout` [*float*, default: **120.0**]: time in **[s]**, after which animation will be removed from the queue. + +### Animation Types + +#### Animation + +Basic animation definition. Keys are inherited from the basic **Animation** class by all animations. Supported keys are: + +- `brightness` [*float*, default: **1.0**]: animation brightness relative to APA102C driver `global_brightness`. The range between **[0.0, 1.0]**. +- `duration` [*float*, default: **None**]: duration of the animation. +- `repeat` [*int*, default: **1**]: number of times the animation will be repeated. + +> [!NOTE] +> Overall display duration of an animation is a product of a single image duration and repeat count. The result of `duration` x `repeat` can't exceed 10 **[s]**. If animation fails to fulfill the requirement, it will result in an error. + +#### ImageAnimation + +Animation of type `panther_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). + +### Defining Animations + +Users can define their own LED animations using basic animation types. Similar to basic ones, user animations are parsed using YAML file and loaded on node start. For `ImageAnimation` you can use basic images from the `animations` folder and change their color with the `color` key ([see ImageAnimation](#imageanimation)). Follow the example below to add custom animations. + +Create a YAML file with an animation description list. Example file: + +```yaml +# my_awesome_user_animations.yaml +user_animations: + # animation with default image and custom color + - id: 21 + name: ANIMATION_1 + priority: 2 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/strip01_red.png + duration: 2 + repeat: 2 + color: 0xffff00 + + # animation with custom image + - id: 22 + name: ANIMATION_2 + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: /animations/custom_image.png + duration: 3 + repeat: 1 + + # animation with a custom image from custom ROS package + - id: 23 + name: ANIMATION_3 + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find my_custom_animation_package)/animations/custom_image.png + duration: 3 + repeat: 1 + + # different animations for Front and Rear Bumper Light + - id: 24 + name: ANIMATION_4 + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: front + animation: + image: $(find panther_lights)/animations/triangle01_blue.png + duration: 2 + repeat: 2 + - type: panther_lights::ImageAnimation + segments: rear + animation: + image: $(find panther_lights)/animations/triangle01_red.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**. + +Remember to modify launch command to use user animations: + +``` bash +ros2 launch panther_bringup bringup.launch user_animations_file:=/my_awesome_user_animations.yaml +``` + +Test new animations: + +```bash +ros2 service call /lights/set_animation panther_msgs/srv/SetLEDAnimation "{animation: {id: 0, param: ''}, repeating: true}" +``` + +## Defining a Custom Animation Type + +It is possible to define your own animation type with expected, new behavior. For more information, see: [**Animation API**](LIGHTS_API.md). diff --git a/panther_lights/README.md b/panther_lights/README.md index 0d40da71b..7610e7b58 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -1,284 +1,61 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_lights -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - Package used to control the Husarion Panther Bumper Lights. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) - -## ROS Nodes - -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +## Launch files -### lights_driver_node +This package contains: -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers +- `lights.launch.py`: Responsible for launching the nodes required to control the Panther Bumper Lights. -[//]: # (ROS_API_NODE_PUBLISHERS_START) +## Configuration Files -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: lights diagnostic messages. +- [`led_config.yaml`](./config/led_config.yaml): Defines and describes the appearance and parameters of the animations. -[//]: # (ROS_API_NODE_PUBLISHERS_END) +## ROS Nodes -#### Subscribers +### ControllerNode -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) +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. -- `lights/channel_1_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Front Bumper Lights. -- `lights/channel_2_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Rear Bumper Lights. +#### Publishers -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) +- `lights/channel_1_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: An animation frame to be displayed on robot Front Bumper Lights. +- `lights/channel_2_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: An animation frame to be displayed on robot Rear Bumper Lights. #### Service Servers -[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) - -- `lights/set_brightness` [*panther_msgs/SetLEDBrightness*]: allows setting global LED brightness, value ranges from **0.0** to **1.0**. - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) - -#### Service Clients - -[//]: # (ROS_API_NODE_SERVICE_CLIENTS_START) - -- `hardware/led_control_enable` [*std_srvs/SetBool*]: makes SBC controlling LEDs. - -[//]: # (ROS_API_NODE_SERVICE_CLIENTS_END) +- `lights/set_animation` [*panther_msgs/SetLEDAnimation*]: Allows setting animation on Bumper Lights based on animation ID. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - -- `~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. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +- `~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. -### lights_controller_node +### DriverNode -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -This node is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) +This node is of type rclcpp_components is responsible for displaying frames on the Husarion Panther robot's Bumper Lights. #### Publishers -[//]: # (ROS_API_NODE_PUBLISHERS_START) +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Lights diagnostic messages. -- `lights/channel_1_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Front Bumper Lights. -- `lights/channel_2_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Rear Bumper Lights. +#### Subscribers -[//]: # (ROS_API_NODE_PUBLISHERS_END) +- `lights/channel_1_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: Frame to be displayed on robot Front Bumper Lights. +- `lights/channel_2_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: Frame to be displayed on robot Rear Bumper Lights. #### Service Servers -[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) +- `lights/set_brightness` [*panther_msgs/SetLEDBrightness*]: Allows setting global LED brightness, value ranges from **0.0** to **1.0**. -- `lights/set_animation` [*panther_msgs/SetLEDAnimation*]: allows setting animation on Bumper Lights based on animation ID. +#### Service Clients -[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) +- `hardware/led_control_enable` [*std_srvs/SetBool*]: Makes SBC controlling LEDs. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - -- `~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. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) -[//]: # (ROS_API_PACKAGE_END) - -## LED configuration - -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. - -### Panels - -The `panels` section of the YAML file lists all the physical LED panels on the robot. Each panel has two attributes: - -- `channel` [*int*, default: **None**] the identifier for the LED panel. It is used to differentiate between multiple panels. -- `number_of_leds`: defines the total number of LEDs present on the panel. - -### Segments - -The `segments` section is used to create virtual segments on the robot by dividing the LED panels into different parts. This allows for more precise control over which LEDs are lit up for different effects or indicators. Each segment has three attributes: - -- `name`: the identifier for the segment, such as "front" or "rear". It is used to differentiate between multiple segments. -- `channel`: This specifies which LED panel the segment belongs to. It have to match one of the channels defined in the `panels` section. -- `led_range`: This defines the range of LEDs within the panel that the segment covers. The range is specified as a start-end pair (e.g. 0-45). The range can be specified in reverse order (e.g. 45-0), which may be useful for wiring or orientation reasons. - -### Segments map - -The `segments_map` section allows creating named groups of segments on which animations can be displayed. Each entry under `segments_map` consists of a key representing the group name and a list of segments included in the group. Segment names have to match one of the segments defined in the `segments` section. By default you can use provided mapping: - -- `all` [*list*, default: **None**]: Grouping both `front` and `rear` segments together. -- `front` [*list*, default: **None**]: Containing only the `front` segment. -- `rear` [*list*, default: **None**]: Containing only the `rear` segment. - -### Animations - -The `led_animations` section contains 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`. - - `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. -- `name` [*string*, default: **ANIMATION_**]: name of an animation. If not provided will default to **ANIMATION_**, where `` is equal to `id` parameter of the given animation. -- `priority` [*int*, default: **3**]: priority at which animation will be placed in the queue. The list below shows the behavior when an animation with a given ID arrives: - - **1** interrupts and removes animation with priorities **2** and **3**. - - **2** interrupts animations with priority **3**. - - **3** adds animation to the end of the queue. -- `timeout` [*float*, default: **120.0**]: time in **[s]**, after which animation will be removed from the queue. - -Default animations can be found in the table below: - -| ID | NAME | PRIORITY | ANIMATION | -| :---: | ----------------- | :------: | --------------------------------------------------------------------------------------------------------------------- | -| 0 | E_STOP | 3 | | -| 1 | READY | 3 | | -| 2 | ERROR | 1 | | -| 3 | MANUAL_ACTION | 3 | | -| 4 | AUTONOMOUS_ACTION | 3 | | -| 5 | GOAL_ACHIEVED | 2 | | -| 6 | LOW_BATTERY | 2 | | -| 7 | CRITICAL_BATTERY | 2 | | -| 9 | CHARGING_BATTERY | 3 | | - -### Animation Types - -#### Animation - -Basic animation definition. Keys are inherited from the basic **Animation** class by all animations. Supported keys are: - -- `brightness` [*float*, default: **1.0**]: animation brightness relative to APA102C driver `global_brightness`. The range between **[0.0, 1.0]**. -- `duration` [*float*, default: **None**]: duration of the animation. -- `repeat` [*int*, default: **1**]: number of times the animation will be repeated. - -> [!NOTE] -> Overall display duration of an animation is a product of a single image duration and repeat count. The result of `duration` x `repeat` can't exceed 10 **[s]**. If animation fails to fulfill the requirement it will result in an error. - -#### ImageAnimation - -Animation of type `panther_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). - -### Defining Animations - -Users can define their own LED animations using basic animation types. Similar to basic ones, user animations are parsed using YAML file and loaded on node start. For `ImageAnimation` you can use basic images from the `animations` folder and change their color with the `color` key ([see ImageAnimation](#imageanimation)). Follow the example below to add custom animations. - -Create a YAML file with an animation description list. Example file: - -```yaml -# my_awesome_user_animations.yaml -user_animations: - # animation with default image and custom color - - id: 21 - name: ANIMATION_1 - priority: 2 - animations: - - type: panther_lights::ImageAnimation - segments: all - animation: - image: $(find panther_lights)/animations/strip01_red.png - duration: 2 - repeat: 2 - color: 0xffff00 - - # animation with custom image - - id: 22 - name: ANIMATION_2 - priority: 3 - animation: - - type: panther_lights::ImageAnimation - segments: all - animation: - image: /animations/custom_image.png - duration: 3 - repeat: 1 - - # animation with a custom image from custom ROS package - - id: 23 - name: ANIMATION_3 - priority: 3 - animation: - - type: panther_lights::ImageAnimation - segments: all - animation: - image: $(find my_custom_animation_package)/animations/custom_image.png - duration: 3 - repeat: 1 - - # different animations for Front and Rear Bumper Light - - id: 24 - name: ANIMATION_4 - priority: 3 - animation: - - type: panther_lights::ImageAnimation - segments: front - animation: - image: $(find panther_lights)/animations/triangle01_blue.png - duration: 2 - repeat: 2 - - type: panther_lights::ImageAnimation - segments: rear - animation: - image: $(find panther_lights)/animations/triangle01_red.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**. - -Remember to modify launch command to use user animations: - -``` bash -ros2 launch panther_bringup bringup.launch user_animations_file:=/my_awesome_user_animations.yaml -``` - -Test new animations: - -```bash -ros2 service call /lights/set_animation panther_msgs/srv/SetLEDAnimation "{animation: {id: 0, param: ''}, repeating: true}" -``` - ---- - -## Defining a Custom Animation Type - -It is possible to define your own animation type with expected, new behavior. For more information, see: [**Animation API**](https://github.com/husarion/panther_ros/panther_lights/LIGHTS_API.md). +- `~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. diff --git a/panther_localization/README.md b/panther_localization/README.md index e5e8e7a72..c4dbb47ba 100644 --- a/panther_localization/README.md +++ b/panther_localization/README.md @@ -6,49 +6,11 @@ The package is responsible for activating mods responsible for fusion of data re This package contains: -- `localization.launch.py` - is responsible for activating EKF filtration along with the necessary dependencies needed to operate GPS +- `localization.launch.py`: Responsible for activating EKF filtration along with the necessary dependencies needed to operate GPS. -### localization.launch.py - Launch Arguments +## Configuration Files -- `fuse_gps` [*bool*, default: **False**]: Include GPS for data fusion. -- `localization_config_path` [*string*, default: **depends on `fuse_gps` and `localization_mode`**]: path to the EKF config file. -- `localization_mode` [*string*, default: **relative**]: Specifies the localization mode: - - `relative` - `odometry/filtered` data is relative to the initial position and orientation. - - `enu` - `odometry/filtered` data is relative to the initial position and ENU (East North Up) orientation. -- `namespace` [*string*, default: **env(ROBOT_NAMESPACE)**]: add namespace to all launched nodes. -- `use_sim` [*bool*, default: **False**]: whether simulation is used. - -### localization.launch.py - Nodes Launched - -- `ekf_filter` [*[robot_localization/ekf_node](https://github.com/cra-ros-pkg/robot_localization/tree/ros2)*]: The Extended Kalman Filter node is designed to fuse odometry data from various sources, including wheel encoders, IMU, and GPS. -- `navsat_transform` [*[robot_localization/navsat_transform_node](https://github.com/cra-ros-pkg/robot_localization/tree/ros2)*]: -It converts raw GPS data into odometry data and publishes corrected GPS positions based on sensor data at a higher frequency. - -#### ekf_filter - Subscriber - -- `cmd_vel` [*geometry_msgs/msg/Twist*]: command velocity value. -- `odometry/wheels` [*nav_msgs/msg/Odometry*]: robot odometry calculated from wheels. -- `imu/data` [*sensor_msgs/msg/Imu*]: filtered IMU data. -- `localization/set_pose` [*geometry_msgs/msg/PoseWithCovarianceStamped*]: allows manually set the state of the filter by sending the pose. -- `/tf` [*tf2_msgs/msg/TFMessage*]: transforms of robot system. - -#### ekf_filter - Publishers - -- `diagnostics` [*diagnostic_msgs/msg/DiagnosticArray*]: diagnostic data. -- `odometry/filtered` [*nav_msgs/msg/Odometry*]: 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. -- `/tf` [*tf2_msgs/msg/TFMessage*]: publishes `odom` to `base_link` transform. - -#### ekf_filter - Service Servers - -- `localization/set_pose` [*robot_localization/srv/SetPose*]: upon request, users can manually set the robot's position and speed. This is useful for resetting positions, e.g. during tests. -- `odometry/filtered/global`[*nav_msgs/msg/Odometry*]: contains information about the position and velocities in relation to the initial position and orientation based on geographic ENU convention (x-east, y-north, z-up). [*nav_msgs/Odometry*]: robot odometry calculated from wheels. - -#### navsat_transform - Subscriber - -- `gps/fix` [*sensor_msgs/msg/NavSatFix*]: raw GPS data. -- `odometry/filtered/global` [*nav_msgs/msg/Odometry*]: odometry to transform to GPS position. - -#### navsat_transform - Publishers - -- `gps/filtered` [*sensor_msgs/msg/NavSatFix*]: GPS position after including sensor data from `ekf_filter`. -- `_odometry/gps` [*nav_msgs/msg/Odometry*]: transformed raw GPS data to odometry format. +- [`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. +- [`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/launch/localization.launch.py b/panther_localization/launch/localization.launch.py index e86bfdcea..cec967008 100644 --- a/panther_localization/launch/localization.launch.py +++ b/panther_localization/launch/localization.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): "fuse_gps", default_value="False", description="Include GPS for data fusion", - choices=["False", "True"], + choices=["True", "true", "False", "false"], ) localization_mode = LaunchConfiguration("localization_mode") @@ -60,7 +60,7 @@ def generate_launch_description(): "use_sim", default_value="False", description="Whether simulation is used.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) mode_prefix = PythonExpression(["'", localization_mode, "_'"]) diff --git a/panther_manager/CONFIGURATION.md b/panther_manager/CONFIGURATION.md new file mode 100644 index 000000000..c6d43b038 --- /dev/null +++ b/panther_manager/CONFIGURATION.md @@ -0,0 +1,211 @@ +# panther_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.yaml +hosts: + # Intel NUC, user computer + - ip: 10.15.20.3 + username: husarion + # Universal robots UR5 + - ip: 10.15.20.4 + username: root + # My Raspberry pi that requires very long shutdown sequence + - ip: 10.15.20.12 + timeout: 40 + username: pi + command: /home/pi/my_long_shutdown_sequence.sh +``` + +To set up a connection with a new User Computer and allow execution of commands, login to the Built-in Computer with `ssh husarion@10.15.20.2`. +Add Built-in Computer's public key to **known_hosts** of a computer you want to shut down automatically: + +``` bash +ssh-copy-id username@10.15.20.XX +``` + +> [!IMPORTANT] +> +> To allow your computer to be shutdown without the sudo password, ssh into it and execute +> (if needed replace **husarion** with username of your choice): +> +> ``` bash +> sudo su +> echo husarion 'ALL=(ALL) NOPASSWD: /sbin/poweroff, /sbin/reboot, /sbin/shutdown' | EDITOR='tee -a' visudo +> ``` + +## 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 state, please refer to the [adc_node](/panther_battery/README.md#battery-statuses) documentation. + +| Power Supply Health | Procedure | +| ------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| GOOD | - | +| UNKNOWN | - | +| OVERHEAT | 1. Turn on the fan.
2. If the Battery temperature is higher than 55.0 **[°C]**, trigger an emergency stop and turn off AUX.
3. If the Battery temperature is higher than 62.0 **[°C]**, shutdown the robot. | +| DEAD | Shutdown the robot. | +| OVERVOLTAGE | 1. Initiate an emergency stop.
2. Display an error animation if the charger is connected. | +| COLD | - | + +> [!NOTE] +> +> 1. The fan exhibits a form of hysteresis, allowing it to be turned off after a delay of at least 60 seconds. +> 2. Once the Panther ROS stack initializes, the fan activates and operates for a duration of approximately 60 seconds. + +## BehaviorTree + +This package contains two main BehaviorTree projects. One is designed for lights handling and the other for safety and system shutdown. +For a BehaviorTree project to work correctly, it must contain a tree with correct names. The names are: `Lights` for lights BT project; `Shutdown` and `Safety` for safety BT project. Files with trees XML descriptions can be shared between projects. Each tree is provided with a set of default blackboard entries (described below), which can be used to specify the behavior of a given tree. + +### Nodes + +#### Actions + +- `CallSetBoolService` - allows calling the standard **std_srvs/SetBool** ROS service. Provided ports are: + - `data` [*input*, *bool*, default: **None**]: service data - **true** or **false** value. + - `service_name` [*input*, *string*, default: **None**]: ROS service name. +- `CallSetLedAnimationService` - allows calling custom type **panther_msgs/SetLEDAnimation** ROS service. The provided ports are: + - `id` [*input*, *unsigned*, default: **None**]: animation ID. + - `param` [*input*, *string*, default: **None**]: optional parameter passed to animation. + - `repeating` [*input*, *bool*, default: **false**]: indicates if the animation should repeat. + - `service_name` [*input*, *string*, default: **None**]: ROS service name. +- `CallTriggerService` - allows calling the standard **std_srvs/Trigger** ROS service. The provided ports are: + - `service_name` [*input*, *string*, default: **None**]: ROS service name. +- `ShutdownHostsFromFile` - allows to shutdown devices based on a YAML file. Returns `SUCCESS` only when a YAML file is valid and the shutdown of all defined hosts was successful. Nodes are processed in a semi-parallel fashion. Every tick of the tree updates the state of a host. This allows some hosts to wait for a SSH response, while others are already pinged and awaiting a full shutdown. If a host is shutdown, it is no longer processed. In the case of a long timeout is used for a given host, other hosts will be processed simultaneously. The provided ports are: + - `shutdown_host_file` [*input*, *string*, default: **None**]: global path to YAML file with hosts to shutdown. +- `ShutdownSingleHost` - allows to shut down a single device. Will return `SUCCESS` only when the device has been successfully shutdown. The provided ports are: + - `command` [*input*, *string*, default: **sudo shutdown now**]: command to execute on shutdown. + - `ip` [*input*, *string*, default: **None**]: IP of the host to shutdown. + - `ping_for_success` [*input*, *bool*, default: **true**]: ping host until it is not available or timeout is reached. + - `port` [*input*, *string*, default: **22**]: SSH communication port. + - `timeout` [*input*, *string*, default: **5.0**]: time in **[s]** to wait for the host to shutdown. Keep in mind that hardware will cut power off after a given time after pressing the power button. Refer to the hardware manual for more information. + - `username` [*input*, *string*, default: **None**]: user to log into while executing the shutdown command. +- `SignalShutdown` - signals shutdown of the robot. The provided ports are: + - `message` [*input*, *string*, default: **None**]: message with reason for robot to shutdown. + +#### Decorators + +- `TickAfterTimeout` - will skip a child until the specified time has passed. It can be used to specify the frequency at which a node or subtree is triggered. The provided ports are: + - `timeout` [*input*, *unsigned*, default: **None**]: time in **[s]** to wait before ticking child again. + +### Trees + +#### Lights + +A tree responsible for scheduling animations displayed on the Bumper Lights based on the Husarion Panther robot's system state. + + +

+ Lights Behavior Tree +

+ +Default blackboard entries: + +- `battery_percent` [*float*, default: **None**]: moving average of the Battery percentage. +- `battery_percent_round` [*string*, default: **None**] Battery percentage rounded to a value specified with `~lights/update_charging_anim_step` parameter and cast to string. +- `battery_status` [*unsigned*, default: **None**]: the current Battery status. +- `charging_anim_percent` [*string*, default: **None**]: the charging animation Battery percentage value, cast to a string. +- `current_anim_id` [*int*, default: **-1**]: ID of currently displayed animation. +- `e_stop_state` [*bool*, default: **None**]: state of E-stop. + +Default constant blackboard entries: + +- `BATTERY_STATE_ANIM_PERIOD` [*float*, default: **120.0**]: refers to `battery_state_anim_period` ROS parameter. +- `CRITICAL_BATTERY_ANIM_PERIOD` [*float*, default: **15.0**]: refers to `critical_battery_anim_period` ROS parameter. +- `CRITICAL_BATTERY_THRESHOLD_PERCENT` [*float*, default: **0.1**]: refers to `critical_battery_threshold_percent` ROS parameter. +- `LOW_BATTERY_ANIM_PERIOD` [*float*, default: **30.0**]: refers to `low_battery_anim_period` ROS parameter. +- `LOW_BATTERY_THRESHOLD_PERCENT` [*float*, default: **0.4**]: refers to `low_battery_threshold_percent` ROS parameter. +- `E_STOP_ANIM_ID` [*unsigned*, value: **0**]: animation ID constant obtained from `panther_msgs::LEDAnimation::E_STOP`. +- `READY_ANIM_ID` [*unsigned*, value: **1**]: animation ID constant obtained from `panther_msgs::LEDAnimation::READY`. +- `ERROR_ANIM_ID` [*unsigned*, value: **2**]: animation ID constant obtained from `panther_msgs::LEDAnimation::ERROR`. +- `MANUAL_ACTION_ANIM_ID` [*unsigned*, value: **3**]: animation ID constant obtained from `panther_msgs::LEDAnimation::MANUAL_ACTION`. +- `AUTONOMOUS_ACTION_ANIM_ID` [*unsigned*, value: **4**]: animation ID constant obtained from `panther_msgs::LEDAnimation::AUTONOMOUS_ACTION`. +- `GOAL_ACHIEVED_ANIM_ID` [*unsigned*, value: **5**]: animation ID constant obtained from `panther_msgs::LEDAnimation::GOAL_ACHIEVED`. +- `LOW_BATTERY_ANIM_ID` [*unsigned*, value: **6**]: animation ID constant obtained from `panther_msgs::LEDAnimation::LOW_BATTERY`. +- `CRITICAL_BATTERY_ANIM_ID` [*unsigned*, value: **7**]: animation ID constant obtained from `panther_msgs::LEDAnimation::CRITICAL_BATTERY`. +- `BATTERY_STATE_ANIM_ID` [*unsigned*, value: **8**]: animation ID constant obtained from `panther_msgs::LEDAnimation::BATTERY_STATE`. +- `CHARGING_BATTERY_ANIM_ID` [*unsigned*, value: **9**]: animation ID constant obtained from `panther_msgs::LEDAnimation::CHARGING_BATTERY`. +- `POWER_SUPPLY_STATUS_UNKNOWN` [*unsigned*, value: **0**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN`. +- `POWER_SUPPLY_STATUS_CHARGING` [*unsigned*, value: **1**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_CHARGING`. +- `POWER_SUPPLY_STATUS_DISCHARGING` [*unsigned*, value: **2**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING`. +- `POWER_SUPPLY_STATUS_NOT_CHARGING` [*unsigned*, value: **3**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING`. +- `POWER_SUPPLY_STATUS_FULL` [*unsigned*, value: **4**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_FULL`. + +### Safety + +A tree responsible for monitoring the Panther robot's state and handling safety measures, such as cooling the robot in case of high Built-in Computer's CPU or Battery temperatures. + + +

+ Safety Behavior Tree +

+ +Default blackboard entries: + +- `aux_state` [*bool*, default: **None**]: state of AUX Power. +- `bat_temp` [*double*, default: **None**]: moving average of the Battery temperature. +- `cpu_temp` [*double*, default: **None**]: moving average of the Built-in Computer's CPU temperature +- `driver_temp` [*double*, default: **None**]: moving average of driver temperature. Out of the two drivers, the one with the higher temperature is taken into account. +- `e_stop_state` [*bool*, default: **None**]: state of the E-stop. +- `fan_state` [*bool*, default: **None**]: state of the fan. + +Default constant blackboard entries: + +- `CPU_FAN_OFF_TEMP` [*float*, default: **60.0**]: refers to the`cpu_fan_off_temp` ROS parameter. +- `CPU_FAN_ON_TEMP` [*float*, default: **70.0**]: refers to the `cpu_fan_on_temp` ROS parameter. +- `CRITICAL_BAT_TEMP` [*float*, default: **59.0**]: refers to the `critical_bat_temp` ROS parameter. +- `DRIVER_FAN_OFF_TEMP` [*float*, default: **35.0**]: refers to the `driver_fan_off_temp` ROS parameter. +- `DRIVER_FAN_ON_TEMP` [*float*, default: **45.0**]: refers to the `driver_fan_on_temp` ROS parameter. +- `HIGH_BAT_TEMP` [*float*, default: **55.0**]: refers to the `high_bat_temp` ROS parameter. +- `POWER_SUPPLY_HEALTH_UNKNOWN` [*unsigned*, value: **0**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN`. +- `POWER_SUPPLY_HEALTH_GOOD` [*unsigned*, value: **1**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_GOOD`. +- `POWER_SUPPLY_HEALTH_OVERHEAT` [*unsigned*, value: **2**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_OVERHEAT`. +- `POWER_SUPPLY_HEALTH_DEAD` [*unsigned*, value: **3**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_DEAD`. +- `POWER_SUPPLY_HEALTH_OVERVOLTAGE` [*unsigned*, value: **4**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_OVERVOLTAGE`. +- `POWER_SUPPLY_HEALTH_UNSPEC_FAILURE` [*unsigned*, value: **5**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNSPEC_FAILURE`. +- `POWER_SUPPLY_HEALTH_COLD` [*unsigned*, value: **6**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_COLD`. +- `POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE` [*unsigned*, value: **7**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE`. +- `POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE` [*unsigned*, value: **8**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE`. + +### Shutdown + +A tree responsible for the graceful shutdown of robot components, user computers, and the Built-in Computer. By default, it will proceed to shutdown all computers defined in a YAML file with a path defined by the `shutdown_host_path` ROS parameter. + + +

+ Shutdown Behavior Tree +

+ +Default constant blackboard entries: + +- `SHUTDOWN_HOSTS_PATH` [*string*, default: **None**]: refers to `shutdown_hosts_path` ROS parameter. + +Expected blackboard entries: + +- `signal_shutdown` [*pair(bool, string)*, default: **(false, '')**]: flag to shutdown robot with information to display while shutting down. + +### Modifying Behavior Trees + +Each behavior tree can be easily customized to enhance its functions and capabilities. To achieve this, we recommend using Groot2, a powerful tool for developing and modifying behavior trees. To install Groot2 and learn how to use it, please refer to the [official guidelines](https://www.behaviortree.dev/groot). + +When creating a new BehaviorTree project, it is advised to use an existing project as a guideline and leverage it for reference. You can study the structure and implementation of the behavior trees in the existing project to inform your own development process. The project should consist of `Lights` behavior tree or both `Safety` and `Shutdown` behavior tree. Additionally, you have the option to incorporate some of the files used in the existing project into your own project. By utilizing these files, you can benefit from the work already done and save time and effort in developing certain aspects of the behavior trees. + +> [!NOTE] +> It is essential to exercise caution when modifying the trees responsible for safety or shutdown and ensure that default behaviors are not removed. +> +> Remember to use the files from the existing project in a way that avoids conflicts, such as by saving them under new names to ensure they don't overwrite any existing files. + +When modifying behavior trees, you have the flexibility to use standard BehaviorTree.CPP nodes or leverage nodes created specifically for Panther, as detailed in the [Nodes](#nodes) section. Additionally, if you have more specific requirements, you can even create your own custom Behavior Tree nodes. However, this will involve modifying the package and rebuilding the project accordingly. + +To use your customized project, you can modify the `bt_project_file` ROS parameter. + +### 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: + +- Lights tree: `10.15.20.2:5555` +- Safety tree: `10.15.20.2:6666` +- Shutdown tree: `10.15.20.2:7777` diff --git a/panther_manager/README.md b/panther_manager/README.md index 70fbdbb63..0f0742ccc 100644 --- a/panther_manager/README.md +++ b/panther_manager/README.md @@ -1,337 +1,88 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_manager -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - A package containing nodes responsible for high-level control of Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) +## Launch Files -## ROS Nodes +This package contains: -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +- `manager_bt.launch.py`: Responsible for launching behavior trees responsible for safety and LED animations scheduling. -### lights_manager +## Configuration Files -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -Node responsible for managing Bumper Lights animation scheduling. +- [`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. +- [`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_config.yaml`](./config/lights_manager_config.yaml): Contains parameters for the `lights_manager` node. +- [`safety_manager_config.yaml`](./config/safety_manager_config.yaml): Contains parameters for the `safety_manager` node. +- [`shutdown_hosts.yaml`](./config/shutdown_hosts.yaml): List with all hosts to request shutdown. -[//]: # (ROS_API_NODE_DESCRIPTION_END) +## ROS Nodes -#### Subscribers +### lights_manager_node -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) +Node responsible for managing Bumper Lights animation scheduling. -- `battery/battery_status` [*sensor_msgs/BatteryState*]: state of the internal Battery. -- `hardware/e_stop` [*std_msgs/Bool*]: state of emergency stop. +#### Subscribers -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) +- `battery/battery_status` [*sensor_msgs/BatteryState*]: State of the internal Battery. +- `hardware/e_stop` [*std_msgs/Bool*]: State of emergency stop. #### Service Clients (for Default Trees) -[//]: # (ROS_API_NODE_SERVICE_CLIENTS_START) - -- `~/lights/set_animation` [*panther_msgs/SetLEDAnimation*]: allows setting animation on Bumper Lights based on animation ID. - -[//]: # (ROS_API_NODE_SERVICE_CLIENTS_END) +- `~/lights/set_animation` [*panther_msgs/SetLEDAnimation*]: Allows setting animation on Bumper Lights based on animation ID. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - -- `battery.anim_period.critical` [*float*, default: **15.0**]: time in **[s]** to wait before repeating animation, indicating a critical Battery state. -- `battery.anim_period.low` [*float*, default: **30.0**]: time in **[s]** to wait before repeating the animation, indicating a low Battery state. -- `battery.charging_anim_step` [*float*, default: **0.1**]: this parameter defines the minimum change in battery percentage required to trigger an update in the battery charging animation. -- `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. -- `plugin_libs` [*list*, default: **Empty list**]: list with names of plugins that are used in the BT project. -- `ros_plugin_libs` [*list*, default: **Empty list**]: list with names of ROS plugins that are used in a BT project. -- `timer_frequency` [*float*, default: **10.0**]: frequency **[Hz]** at which lights tree will be ticked. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +- `battery.anim_period.critical` [*float*, default: **15.0**]: Time in **[s]** to wait before repeating animation, indicating a critical Battery state. +- `battery.anim_period.low` [*float*, default: **30.0**]: Time in **[s]** to wait before repeating the animation, indicating a low Battery state. +- `battery.charging_anim_step` [*float*, default: **0.1**]: This parameter defines the minimum change in battery percentage required to trigger an update in the battery charging animation. +- `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. +- `plugin_libs` [*list*, default: **Empty list**]: List with names of plugins that are used in the BT project. +- `ros_plugin_libs` [*list*, default: **Empty list**]: List with names of ROS plugins that are used in a BT project. +- `timer_frequency` [*float*, default: **10.0**]: Frequency **[Hz]** at which lights tree will be ticked. -### safety_manager - -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) +### safety_manager_node Node responsible for managing safety features, and software shutdown of components. -[//]: # (ROS_API_NODE_DESCRIPTION_END) - #### Subscribers -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `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. -- `system_status` [*panther_msgs/SystemStatus*]: state of the system, including Built-in Computer's CPU temperature and load. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) +- `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. +- `system_status` [*panther_msgs/SystemStatus*]: State of the system, including Built-in Computer's CPU temperature and load. #### Service Clients (for Default Trees) -[//]: # (ROS_API_NODE_SERVICE_CLIENTS_START) - -- `~/hardware/aux_power_enable` [*std_srvs/SetBool*]: enables Aux Power output. -- `~/hardware/e_stop_trigger` [*std_srvs/Trigger*]: triggers E-stop. -- `~/hardware/fan_enable` [*std_srvs/SetBool*]: enables fan. - -[//]: # (ROS_API_NODE_SERVICE_CLIENTS_END) +- `~/hardware/aux_power_enable` [*std_srvs/SetBool*]: Enables Aux Power output. +- `~/hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop. +- `~/hardware/fan_enable` [*std_srvs/SetBool*]: Enables fan. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - -- `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. -- `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. -- `driver.temp.fan_off` [*float*, default: **35.0**]: temperature in **[°C]** of any drivers below which the fan is turned off. -- `driver.temp.fan_on` [*float*, default: **45.0**]: temperature in **[°C]** of any drivers above which the fan is turned on. -- `driver.temp.window_len` [*int*, default: **6**]: moving average window length used to smooth out the temperature readings of each driver. -- `fan_turn_off_timeout` [*float*, default: **60.0**]: minimal time in **[s]**, after which the fan may be turned off. -- `plugin_libs` [*list*, default: **Empty list**]: list with names of plugins that are used in the BT project. -- `ros_plugin_libs` [*list*, default: **Empty list**]: list with names of ROS plugins that are used in a BT project. -- `shutdown_hosts_path` [*string*, default: **None**]: path to a YAML file containing a list of hosts to request shutdown. To correctly format the YAML file, include a **hosts** field consisting of a list with the following fields: - - `command` [*string*, default: **sudo shutdown now**]: command executed on shutdown of given device. +- `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. +- `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. +- `driver.temp.fan_off` [*float*, default: **35.0**]: Temperature in **[°C]** of any drivers below which the fan is turned off. +- `driver.temp.fan_on` [*float*, default: **45.0**]: Temperature in **[°C]** of any drivers above which the fan is turned on. +- `driver.temp.window_len` [*int*, default: **6**]: Moving average window length used to smooth out the temperature readings of each driver. +- `fan_turn_off_timeout` [*float*, default: **60.0**]: Minimal time in **[s]**, after which the fan may be turned off. +- `plugin_libs` [*list*, default: **Empty list**]: List with names of plugins that are used in the BT project. +- `ros_plugin_libs` [*list*, default: **Empty list**]: List with names of ROS plugins that are used in a BT project. +- `shutdown_hosts_path` [*string*, default: **None**]: Path to a YAML file containing a list of hosts to request shutdown. To correctly format the YAML file, include a **hosts** field consisting of a list with the following fields: + - `command` [*string*, default: **sudo shutdown now**]: Command executed on shutdown of given device. - `ip` [*string*, default: **None**]: IP of a host to shutdown over SSH. - - `ping_for_success` [*bool*, default: **true**]: ping host until it is not available or timeout is reached. + - `ping_for_success` [*bool*, default: **true**]: Ping host until it is not available or timeout is reached. - `port` [*string*, default: **22**]: SSH communication port. - - `timeout` [*string*, default: **5.0**]: time in **[s]** to wait for the host to shutdown. The Built-in Computer will turn off after all computers are shutdown or reached timeout. Keep in mind that hardware will cut power off after a given time after pressing the power button. Refer to the hardware manual for more information. - - `username` [*string*, default: **None**]: username used to log in to over SSH. -- `timer_frequency` [*float*, default: **10.0**]: frequency **[Hz]** at which safety tree will be ticked. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) -[//]: # (ROS_API_PACKAGE_END) - -#### 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.yaml -hosts: - # Intel NUC, user computer - - ip: 10.15.20.3 - username: husarion - # Universal robots UR5 - - ip: 10.15.20.4 - username: root - # My Raspberry pi that requires very long shutdown sequence - - ip: 10.15.20.12 - timeout: 40 - username: pi - command: /home/pi/my_long_shutdown_sequence.sh -``` - -To set up a connection with a new User Computer and allow execution of commands, login to the Built-in Computer with `ssh husarion@10.15.20.2`. -Add Built-in Computer's public key to **known_hosts** of a computer you want to shutdown automatically: - -``` bash -ssh-copy-id username@10.15.20.XX -``` - -> **Warning** -> To allow your computer to be shutdown without the sudo password, ssh into it and execute -> (if needed replace **husarion** with username of your choice): -> -> ``` bash -> sudo su -> echo husarion 'ALL=(ALL) NOPASSWD: /sbin/poweroff, /sbin/reboot, /sbin/shutdown' | EDITOR='tee -a' visudo -> ``` - -#### 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 state, please refer to the [adc_node](/panther_battery/README.md#battery-statuses) documentation. - -| Power Supply Health | Procedure | -| ------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| GOOD | - | -| UNKNOWN | - | -| OVERHEAT | 1. Turn on the fan.
2. If the Battery temperature is higher than 55.0 **[°C]**, trigger an emergency stop and turn off AUX.
3. If the Battery temperature is higher than 62.0 **[°C]**, shutdown the robot. | -| DEAD | Shutdown the robot. | -| OVERVOLTAGE | 1. Initiate an emergency stop.
2. Display an error animation if the charger is connected. | -| COLD | - | - -> **NOTE** -> -> 1. The fan exhibits a form of hysteresis, allowing it to be turned off after a delay of at least 60 seconds. -> 2. Once the Panther ROS stack initializes, the fan activates and operates for a duration of approximately 60 seconds. - ---- - -## BehaviorTree - -This package contains two main BehaviorTree projects. One is designed for lights handling and the other for safety and system shutdown. -For a BehaviorTree project to work correctly, it must contain a tree with correct names. The names are: `Lights` for lights BT project; `Shutdown` and `Safety` for safety BT project. Files with trees XML descriptions can be shared between projects. Each tree is provided with a set of default blackboard entries (described below), which can be used to specify the behavior of a given tree. - -### Nodes - -#### Actions - -- `CallSetBoolService` - allows calling the standard **std_srvs/SetBool** ROS service. Provided ports are: - - `data` [*input*, *bool*, default: **None**]: service data - **true** or **false** value. - - `service_name` [*input*, *string*, default: **None**]: ROS service name. -- `CallSetLedAnimationService` - allows calling custom type **panther_msgs/SetLEDAnimation** ROS service. The provided ports are: - - `id` [*input*, *unsigned*, default: **None**]: animation ID. - - `param` [*input*, *string*, default: **None**]: optional parameter passed to animation. - - `repeating` [*input*, *bool*, default: **false**]: indicates if the animation should repeat. - - `service_name` [*input*, *string*, default: **None**]: ROS service name. -- `CallTriggerService` - allows calling the standard **std_srvs/Trigger** ROS service. The provided ports are: - - `service_name` [*input*, *string*, default: **None**]: ROS service name. -- `ShutdownHostsFromFile` - allows to shutdown devices based on a YAML file. Returns `SUCCESS` only when a YAML file is valid and the shutdown of all defined hosts was successful. Nodes are processed in a semi-parallel fashion. Every tick of the tree updates the state of a host. This allows some hosts to wait for a SSH response, while others are already pinged and awaiting a full shutdown. If a host is shutdown it is no longer processed. In the case of a long timeout is used for a given host, other hosts will be processed simultaneously. The provided ports are: - - `shutdown_host_file` [*input*, *string*, default: **None**]: global path to YAML file with hosts to shutdown. -- `ShutdownSingleHost` - allows to shutdown a single device. Will return `SUCCESS` only when the device has been successfully shutdown. The provided ports are: - - `command` [*input*, *string*, default: **sudo shutdown now**]: command to execute on shutdown. - - `ip` [*input*, *string*, default: **None**]: IP of the host to shutdown. - - `ping_for_success` [*input*, *bool*, default: **true**]: ping host until it is not available or timeout is reached. - - `port` [*input*, *string*, default: **22**]: SSH communication port. - - `timeout` [*input*, *string*, default: **5.0**]: time in **[s]** to wait for the host to shutdown. Keep in mind that hardware will cut power off after a given time after pressing the power button. Refer to the hardware manual for more information. - - `username` [*input*, *string*, default: **None**]: user to log into while executing the shutdown command. -- `SignalShutdown` - signals shutdown of the robot. The provided ports are: - - `message` [*input*, *string*, default: **None**]: message with reason for robot to shutdown. - -#### Decorators - -- `TickAfterTimeout` - will skip a child until the specified time has passed. It can be used to specify the frequency at which a node or subtree is triggered. The provided ports are: - - `timeout` [*input*, *unsigned*, default: **None**]: time in **[s]** to wait before ticking child again. - -### Trees - -#### Lights - -A tree responsible for scheduling animations displayed on the Bumper Lights based on the Husarion Panther robot's system state. - - -

- -

- -Default blackboard entries: - -- `battery_percent` [*float*, default: **None**]: moving average of the Battery percentage. -- `battery_percent_round` [*string*, default: **None**] Battery percentage rounded to a value specified with `~lights/update_charging_anim_step` parameter and cast to string. -- `battery_status` [*unsigned*, default: **None**]: the current Battery status. -- `charging_anim_percent` [*string*, default: **None**]: the charging animation Battery percentage value, cast to a string. -- `current_anim_id` [*int*, default: **-1**]: ID of currently displayed animation. -- `e_stop_state` [*bool*, default: **None**]: state of E-stop. - -Default constant blackboard entries: - -- `BATTERY_STATE_ANIM_PERIOD` [*float*, default: **120.0**]: refers to `battery_state_anim_period` ROS parameter. -- `CRITICAL_BATTERY_ANIM_PERIOD` [*float*, default: **15.0**]: refers to `critical_battery_anim_period` ROS parameter. -- `CRITICAL_BATTERY_THRESHOLD_PERCENT` [*float*, default: **0.1**]: refers to `critical_battery_threshold_percent` ROS parameter. -- `LOW_BATTERY_ANIM_PERIOD` [*float*, default: **30.0**]: refers to `low_battery_anim_period` ROS parameter. -- `LOW_BATTERY_THRESHOLD_PERCENT` [*float*, default: **0.4**]: refers to `low_battery_threshold_percent` ROS parameter. -- `E_STOP_ANIM_ID` [*unsigned*, value: **0**]: animation ID constant obtained from `panther_msgs::LEDAnimation::E_STOP`. -- `READY_ANIM_ID` [*unsigned*, value: **1**]: animation ID constant obtained from `panther_msgs::LEDAnimation::READY`. -- `ERROR_ANIM_ID` [*unsigned*, value: **2**]: animation ID constant obtained from `panther_msgs::LEDAnimation::ERROR`. -- `MANUAL_ACTION_ANIM_ID` [*unsigned*, value: **3**]: animation ID constant obtained from `panther_msgs::LEDAnimation::MANUAL_ACTION`. -- `AUTONOMOUS_ACTION_ANIM_ID` [*unsigned*, value: **4**]: animation ID constant obtained from `panther_msgs::LEDAnimation::AUTONOMOUS_ACTION`. -- `GOAL_ACHIEVED_ANIM_ID` [*unsigned*, value: **5**]: animation ID constant obtained from `panther_msgs::LEDAnimation::GOAL_ACHIEVED`. -- `LOW_BATTERY_ANIM_ID` [*unsigned*, value: **6**]: animation ID constant obtained from `panther_msgs::LEDAnimation::LOW_BATTERY`. -- `CRITICAL_BATTERY_ANIM_ID` [*unsigned*, value: **7**]: animation ID constant obtained from `panther_msgs::LEDAnimation::CRITICAL_BATTERY`. -- `BATTERY_STATE_ANIM_ID` [*unsigned*, value: **8**]: animation ID constant obtained from `panther_msgs::LEDAnimation::BATTERY_STATE`. -- `CHARGING_BATTERY_ANIM_ID` [*unsigned*, value: **9**]: animation ID constant obtained from `panther_msgs::LEDAnimation::CHARGING_BATTERY`. -- `POWER_SUPPLY_STATUS_UNKNOWN` [*unsigned*, value: **0**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN`. -- `POWER_SUPPLY_STATUS_CHARGING` [*unsigned*, value: **1**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_CHARGING`. -- `POWER_SUPPLY_STATUS_DISCHARGING` [*unsigned*, value: **2**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING`. -- `POWER_SUPPLY_STATUS_NOT_CHARGING` [*unsigned*, value: **3**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING`. -- `POWER_SUPPLY_STATUS_FULL` [*unsigned*, value: **4**]: power supply status constant obtained from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_FULL`. - -### Safety - -A tree responsible for monitoring the Panther robot's state and handling safety measures, such as cooling the robot in case of high Built-in Computer's CPU or Battery temperatures. - - -

- -

- -Default blackboard entries: - -- `aux_state` [*bool*, default: **None**]: state of AUX Power. -- `bat_temp` [*double*, default: **None**]: moving average of the Battery temperature. -- `cpu_temp` [*double*, default: **None**]: moving average of the Built-in Computer's CPU temperature -- `driver_temp` [*double*, default: **None**]: moving average of driver temperature. Out of the two drivers, the one with the higher temperature is taken into account. -- `e_stop_state` [*bool*, default: **None**]: state of the E-stop. -- `fan_state` [*bool*, default: **None**]: state of the fan. - -Default constant blackboard entries: - -- `CPU_FAN_OFF_TEMP` [*float*, default: **60.0**]: refers to the`cpu_fan_off_temp` ROS parameter. -- `CPU_FAN_ON_TEMP` [*float*, default: **70.0**]: refers to the `cpu_fan_on_temp` ROS parameter. -- `CRITICAL_BAT_TEMP` [*float*, default: **59.0**]: refers to the `critical_bat_temp` ROS parameter. -- `DRIVER_FAN_OFF_TEMP` [*float*, default: **35.0**]: refers to the `driver_fan_off_temp` ROS parameter. -- `DRIVER_FAN_ON_TEMP` [*float*, default: **45.0**]: refers to the `driver_fan_on_temp` ROS parameter. -- `HIGH_BAT_TEMP` [*float*, default: **55.0**]: refers to the `high_bat_temp` ROS parameter. -- `POWER_SUPPLY_HEALTH_UNKNOWN` [*unsigned*, value: **0**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN`. -- `POWER_SUPPLY_HEALTH_GOOD` [*unsigned*, value: **1**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_GOOD`. -- `POWER_SUPPLY_HEALTH_OVERHEAT` [*unsigned*, value: **2**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_OVERHEAT`. -- `POWER_SUPPLY_HEALTH_DEAD` [*unsigned*, value: **3**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_DEAD`. -- `POWER_SUPPLY_HEALTH_OVERVOLTAGE` [*unsigned*, value: **4**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_OVERVOLTAGE`. -- `POWER_SUPPLY_HEALTH_UNSPEC_FAILURE` [*unsigned*, value: **5**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNSPEC_FAILURE`. -- `POWER_SUPPLY_HEALTH_COLD` [*unsigned*, value: **6**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_COLD`. -- `POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE` [*unsigned*, value: **7**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE`. -- `POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE` [*unsigned*, value: **8**]: power supply status constant obtained from the `sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE`. - -### Shutdown - -A tree responsible for the graceful shutdown of robot components, user computers, and the Built-in Computer. By default, it will proceed to shutdown all computers defined in a YAML file with a path defined by the `shutdown_host_path` ROS parameter. - - -

- -

- -Default constant blackboard entries: - -- `SHUTDOWN_HOSTS_PATH` [*string*, default: **None**]: refers to `shutdown_hosts_path` ROS parameter. - -Expected blackboard entries: - -- `signal_shutdown` [*pair(bool, string)*, default: **(false, '')**]: flag to shutdown robot with information to display while shutting down. - -### Modifying Behavior Trees - -Each behavior tree can be easily customized to enhance its functions and capabilities. To achieve this, we recommend using Groot2, a powerful tool for developing and modifying behavior trees. To install Groot2 and learn how to use it, please refer to the [official guidelines](https://www.behaviortree.dev/groot). - -When creating a new BehaviorTree project, it is advised to use an existing project as a guideline and leverage it for reference. You can study the structure and implementation of the behavior trees in the existing project to inform your own development process. The project should consist of `Lights` behavior tree or both `Safety` and `Shutdown` behavior tree. Additionally, you have the option to incorporate some of the files used in the existing project into your own project. By utilizing these files, you can benefit from the work already done and save time and effort in developing certain aspects of the behavior trees. - -> **Note** -> It is essential to exercise caution when modifying the trees responsible for safety or shutdown and ensure that default behaviors are not removed. -> -> Remember to use the files from the existing project in a way that avoids conflicts, such as by saving them under new names to ensure they don't overwrite any existing files. - -When modifying behavior trees, you have the flexibility to use standard BehaviorTree.CPP nodes or leverage nodes created specifically for Panther, as detailed in the [Nodes](#nodes) section. Additionally, if you have more specific requirements, you can even create your own custom Behavior Tree nodes. However, this will involve modifying the package and rebuilding the project accordingly. - -To use your customized project, you can modify the `bt_project_file` ROS parameter. - -### 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: - -- Lights tree: `10.15.20.2:5555` -- Safety tree: `10.15.20.2:6666` -- Shutdown tree: `10.15.20.2:7777` + - `timeout` [*string*, default: **5.0**]: Time in **[s]** to wait for the host to shutdown. The Built-in Computer will turn off after all computers are shutdown or reached timeout. Keep in mind that hardware will cut power off after a given time after pressing the power button. Refer to the hardware manual for more information. + - `username` [*string*, default: **None**]: Username used to log in to over SSH. +- `timer_frequency` [*float*, default: **10.0**]: Frequency **[Hz]** at which safety tree will be ticked. diff --git a/panther_bringup/config/shutdown_hosts.yaml b/panther_manager/config/shutdown_hosts.yaml similarity index 100% rename from panther_bringup/config/shutdown_hosts.yaml rename to panther_manager/config/shutdown_hosts.yaml diff --git a/panther_manager/launch/manager_bt.launch.py b/panther_manager/launch/manager_bt.launch.py index 0e3b23b3e..0eecf65ed 100644 --- a/panther_manager/launch/manager_bt.launch.py +++ b/panther_manager/launch/manager_bt.launch.py @@ -63,7 +63,7 @@ def generate_launch_description(): "shutdown_hosts_config_path", default_value=PathJoinSubstitution( [ - FindPackageShare("panther_bringup"), + FindPackageShare("panther_manager"), "config", "shutdown_hosts.yaml", ] From d2f016891c89079c1740b8a71c10eb41ae929580 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 5 Aug 2024 12:11:16 +0200 Subject: [PATCH 04/67] Add EStop to Gazebo --- panther_description/urdf/gazebo.urdf.xacro | 1 + .../urdf/panther_macro.urdf.xacro | 4 +- panther_gazebo/CMakeLists.txt | 57 +- panther_gazebo/config/led_strips.yaml | 4 +- .../panther_gazebo/gz_panther_system.hpp | 113 +++ panther_gazebo/panther_hardware_plugins.xml | 10 + panther_gazebo/src/gz_panther_system.cpp | 843 ++++++++++++++++++ 7 files changed, 1027 insertions(+), 5 deletions(-) create mode 100644 panther_gazebo/include/panther_gazebo/gz_panther_system.hpp create mode 100644 panther_gazebo/panther_hardware_plugins.xml create mode 100644 panther_gazebo/src/gz_panther_system.cpp diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro index b8b2afdbb..3884e7ee6 100644 --- a/panther_description/urdf/gazebo.urdf.xacro +++ b/panther_description/urdf/gazebo.urdf.xacro @@ -42,6 +42,7 @@ ${config_file} ${namespace} + gz_ros2_control/e_stop:=hardware/e_stop imu_broadcaster/imu:=imu/data drive_controller/cmd_vel_unstamped:=cmd_vel drive_controller/odom:=odometry/wheels diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro index 60b90ff09..705bd760a 100644 --- a/panther_description/urdf/panther_macro.urdf.xacro +++ b/panther_description/urdf/panther_macro.urdf.xacro @@ -62,7 +62,9 @@ - ign_ros2_control/IgnitionSystem + panther_gazebo/PantherSystem + 20 + true diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt index 5b53b717b..6287b2684 100644 --- a/panther_gazebo/CMakeLists.txt +++ b/panther_gazebo/CMakeLists.txt @@ -1,22 +1,43 @@ cmake_minimum_required(VERSION 3.10.2) project(panther_gazebo) +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(ament_cmake REQUIRED) -find_package(ignition-transport11 QUIET REQUIRED) +# find_package(ament_index_cpp REQUIRED) +find_package(hardware_interface REQUIRED) find_package(ignition-common4 REQUIRED) +find_package(ignition-gazebo6 REQUIRED) find_package(ignition-msgs8 REQUIRED) +find_package(ignition-plugin1 REQUIRED) +find_package(ignition-transport11 QUIET REQUIRED) +find_package(ign_ros2_control REQUIRED) +find_package(pluginlib REQUIRED) find_package(panther_utils REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(yaml-cpp REQUIRED) include_directories(include) -set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) +set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) +set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) +set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) add_executable(gz_led_strip_manager src/main.cpp src/gz_led_strip_manager.cpp src/gz_led_strip.cpp) @@ -29,4 +50,36 @@ ament_target_dependencies(gz_led_strip_manager panther_utils) install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) +add_library(panther_hardware_plugins SHARED src/gz_panther_system.cpp) +ament_target_dependencies( + panther_hardware_plugins + rclcpp_lifecycle + hardware_interface + rclcpp + std_msgs + std_srvs + ign_ros2_control) +target_link_libraries(panther_hardware_plugins + ignition-gazebo${IGN_GAZEBO_VER}::core) + +install( + TARGETS panther_hardware_plugins + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY include/ DESTINATION include) + +# Testing and linting +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) + +pluginlib_export_plugin_description_file(ign_ros2_control + panther_hardware_plugins.xml) + +# Setup the project ament_package() diff --git a/panther_gazebo/config/led_strips.yaml b/panther_gazebo/config/led_strips.yaml index c5c24c254..c23e06e5f 100644 --- a/panther_gazebo/config/led_strips.yaml +++ b/panther_gazebo/config/led_strips.yaml @@ -1,5 +1,5 @@ chanel_1: - frequency: 15 # Setting to high frequency caused lags in the simulation + frequency: 12 # Setting to high frequency caused lags in the simulation world_name: husarion_world parent_link: panther position: [0.362, 0.0, 0.201] @@ -10,7 +10,7 @@ chanel_1: number_of_leds: 46 chanel_2: - frequency: 15 # Setting to high frequency caused lags in the simulation + frequency: 12 # Setting to high frequency caused lags in the simulation world_name: husarion_world parent_link: panther position: [-0.362, 0.0, 0.201] diff --git a/panther_gazebo/include/panther_gazebo/gz_panther_system.hpp b/panther_gazebo/include/panther_gazebo/gz_panther_system.hpp new file mode 100644 index 000000000..49769afda --- /dev/null +++ b/panther_gazebo/include/panther_gazebo/gz_panther_system.hpp @@ -0,0 +1,113 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_GAZEBO__GZ_PANTHER_SYSTEM +#define PANTHER_GAZEBO__GZ_PANTHER_SYSTEM + +#include +#include +#include +#include + +#include "ign_ros2_control/ign_system_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace panther_gazebo +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +// Forward declaration +class PantherSystemPrivate; + +// These class must inherit `ign_ros2_control::IgnitionSystemInterface` which implements a +// simulated `ros2_control` `hardware_interface::SystemInterface`. + +/// \class PantherSystem +/// \brief Main class for the Panther System which inherits from IgnitionSystemInterface. Based on: +/// https://github.com/ros-controls/gz_ros2_control/blob/humble/ign_ros2_control/src/ign_system.cpp +class PantherSystem : public ign_ros2_control::IgnitionSystemInterface +{ +public: + CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info) override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + hardware_interface::return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + /** + * @brief Initializes the simulation environment for the Panther robot. + * + * This function sets up the internal state of the PantherSystem, configures the simulation + * joints, and registers state and command interfaces based on the provided hardware information. + * It also handles any mimicking of joints and sets the appropriate parameters for the simulation. + * + * @param model_nh Shared pointer to the ROS node handle. + * @param enableJoints Map of joint names to their corresponding Gazebo entities. + * @param hardware_info Struct containing hardware configuration details. + * @param _ecm Entity Component Manager for managing Gazebo entities and components. + * @param update_rate Reference to the simulation update rate. + * + * @return True if initialization is successful, false otherwise. + */ + bool initSim( + rclcpp::Node::SharedPtr & model_nh, std::map & joints, + const hardware_interface::HardwareInfo & hardware_info, + ignition::gazebo::EntityComponentManager & _ecm, int & update_rate) override; + +private: + /// \brief Register a sensor (for now just IMUs) + /// \param[in] hardware_info Hardware information containing sensor data. + void registerSensors(const hardware_interface::HardwareInfo & hardware_info); + + void readParams(const hardware_interface::HardwareInfo & hardware_info); + + void setupEStop(); + + void publishEStopStatus(); + + void eStopResetCallback( + const std::shared_ptr request, + std::shared_ptr response); + + void eStopTriggerCallback( + const std::shared_ptr request, + std::shared_ptr response); + + /// \brief Private data class + std::unique_ptr dataPtr; +}; + +} // namespace panther_gazebo + +#endif // PANTHER_GAZEBO__GZ_PANTHER_SYSTEM diff --git a/panther_gazebo/panther_hardware_plugins.xml b/panther_gazebo/panther_hardware_plugins.xml new file mode 100644 index 000000000..f360a3eb0 --- /dev/null +++ b/panther_gazebo/panther_hardware_plugins.xml @@ -0,0 +1,10 @@ + + + + Control joints position with E-Stop functionaliteis in Gazebo using ROS2. + + + diff --git a/panther_gazebo/src/gz_panther_system.cpp b/panther_gazebo/src/gz_panther_system.cpp new file mode 100644 index 000000000..2266f7060 --- /dev/null +++ b/panther_gazebo/src/gz_panther_system.cpp @@ -0,0 +1,843 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "panther_gazebo/gz_panther_system.hpp" + +struct jointData +{ + /// \brief Joint's names. + std::string name; + + /// \brief Current joint position + double joint_position; + + /// \brief Current joint velocity + double joint_velocity; + + /// \brief Current joint effort + double joint_effort; + + /// \brief Current cmd joint position + double joint_position_cmd; + + /// \brief Current cmd joint velocity + double joint_velocity_cmd; + + /// \brief Current cmd joint effort + double joint_effort_cmd; + + /// \brief flag if joint is actuated (has command interfaces) or passive + bool is_actuated; + + /// \brief handles to the joints from within Gazebo + ignition::gazebo::Entity sim_joint; + + /// \brief Control method defined in the URDF for each joint. + ign_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method; +}; + +struct MimicJoint +{ + std::size_t joint_index; + std::size_t mimicked_joint_index; + double multiplier = 1.0; + std::vector interfaces_to_mimic; +}; + +class ImuData +{ +public: + /// \brief imu's name. + std::string name{}; + + /// \brief imu's topic name. + std::string topicName{}; + + /// \brief handles to the imu from within Gazebo + ignition::gazebo::Entity sim_imu_sensors_ = ignition::gazebo::kNullEntity; + + /// \brief An array per IMU with 4 orientation, 3 angular velocity and 3 linear acceleration + std::array imu_sensor_data_; + + /// \brief callback to get the IMU topic values + void OnIMU(const ignition::msgs::IMU & _msg); +}; + +void ImuData::OnIMU(const ignition::msgs::IMU & _msg) +{ + this->imu_sensor_data_[0] = _msg.orientation().x(); + this->imu_sensor_data_[1] = _msg.orientation().y(); + this->imu_sensor_data_[2] = _msg.orientation().z(); + this->imu_sensor_data_[3] = _msg.orientation().w(); + this->imu_sensor_data_[4] = _msg.angular_velocity().x(); + this->imu_sensor_data_[5] = _msg.angular_velocity().y(); + this->imu_sensor_data_[6] = _msg.angular_velocity().z(); + this->imu_sensor_data_[7] = _msg.linear_acceleration().x(); + this->imu_sensor_data_[8] = _msg.linear_acceleration().y(); + this->imu_sensor_data_[9] = _msg.linear_acceleration().z(); +} + +class panther_gazebo::PantherSystemPrivate +{ +public: + PantherSystemPrivate() = default; + + ~PantherSystemPrivate() = default; + /// \brief Degrees od freedom. + size_t n_dof_; + + /// \brief last time the write method was called. + rclcpp::Time last_update_sim_time_ros_; + + /// \brief vector with the joint's names. + std::vector joints_; + + /// \brief vector with the imus . + std::vector> imus_; + + /// \brief state interfaces that will be exported to the Resource Manager + std::vector state_interfaces_; + + /// \brief command interfaces that will be exported to the Resource Manager + std::vector command_interfaces_; + + /// \brief Entity component manager, ECM shouldn't be accessed outside those + /// methods, otherwise the app will crash + ignition::gazebo::EntityComponentManager * ecm; + + /// \brief controller update rate + int * update_rate; + + /// \brief Ignition communication node. + ignition::transport::Node node; + + /// \brief mapping of mimicked joints to index of joint they mimic + std::vector mimic_joints_; + + /// \brief Gain which converts position error to a velocity command + double position_proportional_gain_; + + bool e_stop_active; + rclcpp::Publisher::SharedPtr e_stop_publisher; + rclcpp::Service::SharedPtr e_stop_reset_service; + rclcpp::Service::SharedPtr e_stop_trigger_service; + rclcpp::TimerBase::SharedPtr e_stop_timer_; + double e_stop_publish_frequency_; +}; + +namespace panther_gazebo +{ +void PantherSystem::readParams(const hardware_interface::HardwareInfo & hardware_info) +{ + if ( + hardware_info.hardware_parameters.find("e_stop_initial_state") != + hardware_info.hardware_parameters.end()) { + std::istringstream(hardware_info.hardware_parameters.at("e_stop_initial_state")) >> + std::boolalpha >> this->dataPtr->e_stop_active; + RCLCPP_INFO( + this->nh_->get_logger(), "Parameter 'e_stop_initial_state' found, using: %s", + this->dataPtr->e_stop_active ? "true" : "false"); + } else { + this->dataPtr->e_stop_active = true; + RCLCPP_INFO( + this->nh_->get_logger(), "Parameter 'e_stop_initial_state' not found, using default true"); + } + if ( + hardware_info.hardware_parameters.find("e_stop_publish_frequency") != + hardware_info.hardware_parameters.end()) { + this->dataPtr->e_stop_publish_frequency_ = + std::stod(hardware_info.hardware_parameters.at("e_stop_publish_frequency")); + RCLCPP_INFO( + this->nh_->get_logger(), "Parameter 'e_stop_publish_frequency' found, using: %f Hz", + this->dataPtr->e_stop_publish_frequency_); + } else { + this->dataPtr->e_stop_publish_frequency_ = 1.0; + RCLCPP_INFO( + this->nh_->get_logger(), + "Parameter 'e_stop_publish_frequency' not found, using default 1.0 Hz"); + } +} + +void PantherSystem::setupEStop() +{ + this->dataPtr->e_stop_publisher = nh_->create_publisher( + "~/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + this->dataPtr->e_stop_reset_service = nh_->create_service( + "~/e_stop_reset", + std::bind( + &PantherSystem::eStopResetCallback, this, std::placeholders::_1, std::placeholders::_2)); + + this->dataPtr->e_stop_trigger_service = nh_->create_service( + "~/e_stop_trigger", + std::bind( + &PantherSystem::eStopTriggerCallback, this, std::placeholders::_1, std::placeholders::_2)); + + auto period = std::chrono::duration(1.0 / this->dataPtr->e_stop_publish_frequency_); + this->dataPtr->e_stop_timer_ = nh_->create_wall_timer( + period, std::bind(&PantherSystem::publishEStopStatus, this)); +} + +void PantherSystem::publishEStopStatus() +{ + std_msgs::msg::Bool e_stop_msg; + e_stop_msg.data = this->dataPtr->e_stop_active; + this->dataPtr->e_stop_publisher->publish(e_stop_msg); +} + +void PantherSystem::eStopResetCallback( + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + this->dataPtr->e_stop_active = false; + response->success = true; + response->message = "E-stop reset"; +} + +void PantherSystem::eStopTriggerCallback( + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + this->dataPtr->e_stop_active = true; + response->success = true; + response->message = "E-stop triggered"; +} + +bool PantherSystem::initSim( + rclcpp::Node::SharedPtr & model_nh, + std::map & enableJoints, + const hardware_interface::HardwareInfo & hardware_info, + ignition::gazebo::EntityComponentManager & _ecm, int & update_rate) +{ + this->dataPtr = std::make_unique(); + this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); + + this->nh_ = model_nh; + this->dataPtr->ecm = &_ecm; + this->dataPtr->n_dof_ = hardware_info.joints.size(); + + this->dataPtr->update_rate = &update_rate; + + RCLCPP_DEBUG(this->nh_->get_logger(), "n_dof_ %lu", this->dataPtr->n_dof_); + + this->dataPtr->joints_.resize(this->dataPtr->n_dof_); + + constexpr double default_gain = 0.1; + + try { + this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter( + "position_proportional_gain", default_gain); + } catch (rclcpp::exceptions::ParameterAlreadyDeclaredException & ex) { + this->nh_->get_parameter( + "position_proportional_gain", this->dataPtr->position_proportional_gain_); + } + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "The position_proportional_gain has been set to: " + << this->dataPtr->position_proportional_gain_); + + if (this->dataPtr->n_dof_ == 0) { + RCLCPP_ERROR_STREAM(this->nh_->get_logger(), "There is no joint available"); + return false; + } + + for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { + auto & joint_info = hardware_info.joints[j]; + std::string joint_name = this->dataPtr->joints_[j].name = joint_info.name; + + auto it = enableJoints.find(joint_name); + if (it == enableJoints.end()) { + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Skipping joint in the URDF named '" + << joint_name << "' which is not in the gazebo model."); + continue; + } + + ignition::gazebo::Entity simjoint = enableJoints[joint_name]; + this->dataPtr->joints_[j].sim_joint = simjoint; + + // Create joint position component if one doesn't exist + if (!_ecm.EntityHasComponentType( + simjoint, ignition::gazebo::components::JointPosition().TypeId())) { + _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointPosition()); + } + + // Create joint velocity component if one doesn't exist + if (!_ecm.EntityHasComponentType( + simjoint, ignition::gazebo::components::JointVelocity().TypeId())) { + _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointVelocity()); + } + + // Create joint force component if one doesn't exist + if (!_ecm.EntityHasComponentType( + simjoint, ignition::gazebo::components::JointForce().TypeId())) { + _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce()); + } + + // Accept this joint and continue configuration + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name); + + std::string suffix = ""; + + // check if joint is mimicked + if (joint_info.parameters.find("mimic") != joint_info.parameters.end()) { + const auto mimicked_joint = joint_info.parameters.at("mimic"); + const auto mimicked_joint_it = std::find_if( + hardware_info.joints.begin(), hardware_info.joints.end(), + [&mimicked_joint](const hardware_interface::ComponentInfo & info) { + return info.name == mimicked_joint; + }); + if (mimicked_joint_it == hardware_info.joints.end()) { + throw std::runtime_error(std::string("Mimicked joint '") + mimicked_joint + "' not found"); + } + + MimicJoint mimic_joint; + mimic_joint.joint_index = j; + mimic_joint.mimicked_joint_index = std::distance( + hardware_info.joints.begin(), mimicked_joint_it); + auto param_it = joint_info.parameters.find("multiplier"); + if (param_it != joint_info.parameters.end()) { + mimic_joint.multiplier = std::stod(joint_info.parameters.at("multiplier")); + } else { + mimic_joint.multiplier = 1.0; + } + + // check joint info of mimicked joint + auto & joint_info_mimicked = hardware_info.joints[mimic_joint.mimicked_joint_index]; + const auto state_mimicked_interface = std::find_if( + joint_info_mimicked.state_interfaces.begin(), joint_info_mimicked.state_interfaces.end(), + [&mimic_joint](const hardware_interface::InterfaceInfo & interface_info) { + bool pos = interface_info.name == "position"; + if (pos) { + mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_POSITION); + } + bool vel = interface_info.name == "velocity"; + if (vel) { + mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_VELOCITY); + } + bool eff = interface_info.name == "effort"; + if (vel) { + mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_EFFORT); + } + return pos || vel || eff; + }); + if (state_mimicked_interface == joint_info_mimicked.state_interfaces.end()) { + throw std::runtime_error( + std::string("For mimic joint '") + joint_info.name + + "' no state interface was found in mimicked joint '" + mimicked_joint + " ' to mimic"); + } + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "Joint '" << joint_name << "'is mimicking joint '" + << mimicked_joint + << "' with multiplier: " << mimic_joint.multiplier); + this->dataPtr->mimic_joints_.push_back(mimic_joint); + suffix = "_mimic"; + } + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); + + auto get_initial_value = + [this, joint_name](const hardware_interface::InterfaceInfo & interface_info) { + double initial_value{0.0}; + if (!interface_info.initial_value.empty()) { + try { + initial_value = std::stod(interface_info.initial_value); + RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", initial_value); + } catch (std::invalid_argument &) { + RCLCPP_ERROR_STREAM( + this->nh_->get_logger(), + "Failed converting initial_value string to real number for the joint " + << joint_name << " and state interface " << interface_info.name + << ". Actual value of parameter: " << interface_info.initial_value + << ". Initial value will be set to 0.0"); + throw std::invalid_argument("Failed converting initial_value string"); + } + } + return initial_value; + }; + + double initial_position = std::numeric_limits::quiet_NaN(); + double initial_velocity = std::numeric_limits::quiet_NaN(); + double initial_effort = std::numeric_limits::quiet_NaN(); + + // register the state handles + for (unsigned int i = 0; i < joint_info.state_interfaces.size(); ++i) { + if (joint_info.state_interfaces[i].name == "position") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); + this->dataPtr->state_interfaces_.emplace_back( + joint_name + suffix, hardware_interface::HW_IF_POSITION, + &this->dataPtr->joints_[j].joint_position); + initial_position = get_initial_value(joint_info.state_interfaces[i]); + this->dataPtr->joints_[j].joint_position = initial_position; + } + if (joint_info.state_interfaces[i].name == "velocity") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); + this->dataPtr->state_interfaces_.emplace_back( + joint_name + suffix, hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joints_[j].joint_velocity); + initial_velocity = get_initial_value(joint_info.state_interfaces[i]); + this->dataPtr->joints_[j].joint_velocity = initial_velocity; + } + if (joint_info.state_interfaces[i].name == "effort") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); + this->dataPtr->state_interfaces_.emplace_back( + joint_name + suffix, hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joints_[j].joint_effort); + initial_effort = get_initial_value(joint_info.state_interfaces[i]); + this->dataPtr->joints_[j].joint_effort = initial_effort; + } + } + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:"); + + // register the command handles + for (unsigned int i = 0; i < joint_info.command_interfaces.size(); ++i) { + if (joint_info.command_interfaces[i].name == "position") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); + this->dataPtr->command_interfaces_.emplace_back( + joint_name + suffix, hardware_interface::HW_IF_POSITION, + &this->dataPtr->joints_[j].joint_position_cmd); + if (!std::isnan(initial_position)) { + this->dataPtr->joints_[j].joint_position_cmd = initial_position; + } + } else if (joint_info.command_interfaces[i].name == "velocity") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); + this->dataPtr->command_interfaces_.emplace_back( + joint_name + suffix, hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joints_[j].joint_velocity_cmd); + if (!std::isnan(initial_velocity)) { + this->dataPtr->joints_[j].joint_velocity_cmd = initial_velocity; + } + } else if (joint_info.command_interfaces[i].name == "effort") { + this->dataPtr->joints_[j].joint_control_method |= EFFORT; + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); + this->dataPtr->command_interfaces_.emplace_back( + joint_name + suffix, hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joints_[j].joint_effort_cmd); + if (!std::isnan(initial_effort)) { + this->dataPtr->joints_[j].joint_effort_cmd = initial_effort; + } + } + // independently of existence of command interface set initial value if defined + if (!std::isnan(initial_position)) { + this->dataPtr->joints_[j].joint_position = initial_position; + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[j].sim_joint, + ignition::gazebo::components::JointPositionReset({initial_position})); + } + if (!std::isnan(initial_velocity)) { + this->dataPtr->joints_[j].joint_velocity = initial_velocity; + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[j].sim_joint, + ignition::gazebo::components::JointVelocityReset({initial_velocity})); + } + } + + // check if joint is actuated (has command interfaces) or passive + this->dataPtr->joints_[j].is_actuated = (joint_info.command_interfaces.size() > 0); + } + + registerSensors(hardware_info); + readParams(hardware_info); + setupEStop(); + + return true; +} + +void PantherSystem::registerSensors(const hardware_interface::HardwareInfo & hardware_info) +{ + // Collect gazebo sensor handles + size_t n_sensors = hardware_info.sensors.size(); + std::vector sensor_components_; + + for (unsigned int j = 0; j < n_sensors; j++) { + hardware_interface::ComponentInfo component = hardware_info.sensors[j]; + sensor_components_.push_back(component); + } + // This is split in two steps: Count the number and type of sensor and associate the interfaces + // So we have resize only once the structures where the data will be stored, and we can safely + // use pointers to the structures + + this->dataPtr->ecm->Each( + [&]( + const ignition::gazebo::Entity & _entity, const ignition::gazebo::components::Imu *, + const ignition::gazebo::components::Name * _name) -> bool { + auto imuData = std::make_shared(); + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); + + auto sensorTopicComp = + this->dataPtr->ecm->Component(_entity); + if (sensorTopicComp) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); + } + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); + imuData->name = _name->Data(); + imuData->sim_imu_sensors_ = _entity; + + hardware_interface::ComponentInfo component; + for (auto & comp : sensor_components_) { + if (comp.name == _name->Data()) { + component = comp; + } + } + + static const std::map interface_name_map = { + {"orientation.x", 0}, {"orientation.y", 1}, {"orientation.z", 2}, + {"orientation.w", 3}, {"angular_velocity.x", 4}, {"angular_velocity.y", 5}, + {"angular_velocity.z", 6}, {"linear_acceleration.x", 7}, {"linear_acceleration.y", 8}, + {"linear_acceleration.z", 9}, + }; + + for (const auto & state_interface : component.state_interfaces) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name); + + size_t data_index = interface_name_map.at(state_interface.name); + this->dataPtr->state_interfaces_.emplace_back( + imuData->name, state_interface.name, &imuData->imu_sensor_data_[data_index]); + } + this->dataPtr->imus_.push_back(imuData); + return true; + }); +} + +CallbackReturn PantherSystem::on_init(const hardware_interface::HardwareInfo & system_info) +{ + RCLCPP_WARN(this->nh_->get_logger(), "On init..."); + if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(this->nh_->get_logger(), "System Successfully configured!"); + + return CallbackReturn::SUCCESS; +} + +std::vector PantherSystem::export_state_interfaces() +{ + return std::move(this->dataPtr->state_interfaces_); +} + +std::vector PantherSystem::export_command_interfaces() +{ + return std::move(this->dataPtr->command_interfaces_); +} + +CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + return CallbackReturn::SUCCESS; + return hardware_interface::SystemInterface::on_activate(previous_state); +} + +CallbackReturn PantherSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state) +{ + return CallbackReturn::SUCCESS; + return hardware_interface::SystemInterface::on_deactivate(previous_state); +} + +hardware_interface::return_type PantherSystem::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].sim_joint == ignition::gazebo::v6::kNullEntity) { + continue; + } + + // Get the joint velocity + const auto * jointVelocity = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 + // Get the joint force + // const auto * jointForce = + // _ecm.Component( + // this->dataPtr->sim_joints_[j]); + + // Get the joint position + const auto * jointPositions = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; + this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; + // this->dataPtr->joint_effort_[j] = jointForce->Data()[0]; + } + + for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { + if (this->dataPtr->imus_[i]->topicName.empty()) { + auto sensorTopicComp = + this->dataPtr->ecm->Component( + this->dataPtr->imus_[i]->sim_imu_sensors_); + if (sensorTopicComp) { + this->dataPtr->imus_[i]->topicName = sensorTopicComp->Data(); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "IMU " << this->dataPtr->imus_[i]->name + << " has a topic name: " << sensorTopicComp->Data()); + + this->dataPtr->node.Subscribe( + this->dataPtr->imus_[i]->topicName, &ImuData::OnIMU, this->dataPtr->imus_[i].get()); + } + } + } + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type PantherSystem::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + for (unsigned int j = 0; j < this->dataPtr->joints_.size(); j++) { + for (const std::string & interface_name : stop_interfaces) { + // Clear joint control method bits corresponding to stop interfaces + if ( + interface_name == + (this->dataPtr->joints_[j].name + "/" + hardware_interface::HW_IF_POSITION)) { + this->dataPtr->joints_[j].joint_control_method &= + static_cast(VELOCITY & EFFORT); + } else if ( + interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_VELOCITY)) { + this->dataPtr->joints_[j].joint_control_method &= + static_cast(POSITION & EFFORT); + } else if ( + interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_EFFORT)) { + this->dataPtr->joints_[j].joint_control_method &= + static_cast(POSITION & VELOCITY); + } + } + + // Set joint control method bits corresponding to start interfaces + for (const std::string & interface_name : start_interfaces) { + if ( + interface_name == + (this->dataPtr->joints_[j].name + "/" + hardware_interface::HW_IF_POSITION)) { + this->dataPtr->joints_[j].joint_control_method |= POSITION; + } else if ( + interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_VELOCITY)) { + this->dataPtr->joints_[j].joint_control_method |= VELOCITY; + } else if ( + interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_EFFORT)) { + this->dataPtr->joints_[j].joint_control_method |= EFFORT; + } + } + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type PantherSystem::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + if (this->dataPtr->e_stop_active) { + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].sim_joint == ignition::gazebo::v6::kNullEntity) { + continue; + } + auto vel = this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + if (vel == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, ignition::gazebo::components::JointVelocityCmd({0})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = 0.0; + } + } + return hardware_interface::return_type::OK; + } + + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].sim_joint == ignition::gazebo::v6::kNullEntity) { + continue; + } + + if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { + if (!this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint)) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, ignition::gazebo::components::JointVelocityCmd({0})); + } else { + const auto jointVelCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + *jointVelCmd = ignition::gazebo::components::JointVelocityCmd( + {this->dataPtr->joints_[i].joint_velocity_cmd}); + } + } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) { + // Get error in position + double error; + error = + (this->dataPtr->joints_[i].joint_position - this->dataPtr->joints_[i].joint_position_cmd) * + *this->dataPtr->update_rate; + + // Calculate target velcity + double target_vel = -this->dataPtr->position_proportional_gain_ * error; + + auto vel = this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + if (vel == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, + ignition::gazebo::components::JointVelocityCmd({target_vel})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = target_vel; + } + } else if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { + if (!this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint)) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, ignition::gazebo::components::JointForceCmd({0})); + } else { + const auto jointEffortCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + *jointEffortCmd = + ignition::gazebo::components::JointForceCmd({this->dataPtr->joints_[i].joint_effort_cmd}); + } + } else if (this->dataPtr->joints_[i].is_actuated) { + // Fallback case is a velocity command of zero (only for actuated joints) + double target_vel = 0.0; + auto vel = this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + if (vel == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, + ignition::gazebo::components::JointVelocityCmd({target_vel})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = target_vel; + } + } + } + + // set values of all mimic joints with respect to mimicked joint + for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { + for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { + if (mimic_interface == "position") { + // Get the joint position + double position_mimicked_joint = + this->dataPtr->ecm + ->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint) + ->Data()[0]; + + double position_mimic_joint = this->dataPtr->ecm + ->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint) + ->Data()[0]; + + double position_error = position_mimic_joint - + position_mimicked_joint * mimic_joint.multiplier; + + double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate); + + auto vel = this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + + if (vel == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointVelocityCmd({velocity_sp})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = velocity_sp; + } + } + if (mimic_interface == "velocity") { + // get the velocity of mimicked joint + double velocity_mimicked_joint = + this->dataPtr->ecm + ->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint) + ->Data()[0]; + + if (!this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointVelocityCmd({0})); + } else { + const auto jointVelCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointVelCmd = ignition::gazebo::components::JointVelocityCmd( + {mimic_joint.multiplier * velocity_mimicked_joint}); + } + } + if (mimic_interface == "effort") { + // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 + // Get the joint force + // const auto * jointForce = + // _ecm.Component( + // this->dataPtr->sim_joints_[j]); + if (!this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({0})); + } else { + const auto jointEffortCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointEffortCmd = ignition::gazebo::components::JointForceCmd( + {mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort}); + } + } + } + } + + return hardware_interface::return_type::OK; +} +} // namespace panther_gazebo + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(panther_gazebo::PantherSystem, ign_ros2_control::IgnitionSystemInterface) From 68a51dfe7cd0c64fbd3bd27963f57f484c509302 Mon Sep 17 00:00:00 2001 From: Dawid Date: Mon, 5 Aug 2024 11:59:47 +0000 Subject: [PATCH 05/67] unify CMakeLists.txt files --- panther_battery/CMakeLists.txt | 46 +++++++---------- panther_controller/CMakeLists.txt | 2 +- panther_description/CMakeLists.txt | 1 + panther_diagnostics/CMakeLists.txt | 36 +++++++------- panther_diagnostics/cmake/SuperBuild.cmake | 2 +- panther_gazebo/CMakeLists.txt | 15 +++--- panther_hardware_interfaces/CMakeLists.txt | 58 +++++++++++----------- panther_lights/CMakeLists.txt | 36 +++++++------- panther_localization/CMakeLists.txt | 2 +- panther_manager/CMakeLists.txt | 22 ++++---- panther_utils/CMakeLists.txt | 28 +++++------ 11 files changed, 122 insertions(+), 126 deletions(-) diff --git a/panther_battery/CMakeLists.txt b/panther_battery/CMakeLists.txt index 9bc58a551..37fd6c814 100644 --- a/panther_battery/CMakeLists.txt +++ b/panther_battery/CMakeLists.txt @@ -5,12 +5,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(panther_msgs REQUIRED) -find_package(panther_utils REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) +set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater panther_msgs + panther_utils rclcpp sensor_msgs) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() include_directories(include ${panther_utils_INCLUDE_DIRS}) @@ -23,9 +23,7 @@ add_executable( src/battery_publisher.cpp src/dual_battery_publisher.cpp src/single_battery_publisher.cpp) - -ament_target_dependencies(battery_node diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) +ament_target_dependencies(battery_node ${PACKAGE_DEPENDENCIES}) install(TARGETS battery_node DESTINATION lib/${PROJECT_NAME}) @@ -73,9 +71,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_publisher diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_single_battery_publisher @@ -85,9 +82,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_single_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_single_battery_publisher diagnostic_updater - panther_msgs panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_single_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_dual_battery_publisher @@ -97,9 +93,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_dual_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_dual_battery_publisher diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_dual_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_battery_node @@ -114,9 +109,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_node PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_node + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_battery_node_roboteq @@ -131,9 +125,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_node_roboteq PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node_roboteq diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_node_roboteq + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_battery_node_dual_bat @@ -148,9 +141,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_node_dual_bat PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node_dual_bat diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_node_dual_bat + ${PACKAGE_DEPENDENCIES}) endif() diff --git a/panther_controller/CMakeLists.txt b/panther_controller/CMakeLists.txt index b71172368..c6489681f 100644 --- a/panther_controller/CMakeLists.txt +++ b/panther_controller/CMakeLists.txt @@ -3,6 +3,6 @@ project(panther_controller) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_description/CMakeLists.txt b/panther_description/CMakeLists.txt index 741a0a391..b0c336d73 100644 --- a/panther_description/CMakeLists.txt +++ b/panther_description/CMakeLists.txt @@ -8,4 +8,5 @@ install(DIRECTORY config launch meshes rviz urdf ament_environment_hooks( "${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in") + ament_package() diff --git a/panther_diagnostics/CMakeLists.txt b/panther_diagnostics/CMakeLists.txt index 6c73bb2f4..c94d1c1e4 100644 --- a/panther_diagnostics/CMakeLists.txt +++ b/panther_diagnostics/CMakeLists.txt @@ -15,25 +15,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_INCLUDE_DEPENDS +set(PACKAGE_DEPENDENCIES ament_cmake - rclcpp - panther_msgs - panther_utils - diagnostic_updater diagnostic_msgs - std_msgs + diagnostic_updater generate_parameter_library - PkgConfig) + panther_msgs + panther_utils + PkgConfig + rclcpp + std_msgs) -foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) endforeach() include_directories(include) set(CPPUPROFILE_PREFIX ${CMAKE_BINARY_DIR}/ep_cppuprofile/src/ep_cppuprofile) set(ENV{PKG_CONFIG_PATH} "${CPPUPROFILE_PREFIX}/lib:$ENV{PKG_CONFIG_PATH}") + pkg_check_modules(CPPUPROFILE REQUIRED IMPORTED_TARGET cppuprofile) generate_parameter_library(system_status_parameters @@ -42,26 +43,25 @@ generate_parameter_library(system_status_parameters add_executable(system_status_node src/main.cpp src/system_status_node.cpp) target_include_directories(system_status_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include) - -ament_target_dependencies(system_status_node ${PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(system_status_node ${PACKAGE_DEPENDENCIES}) target_link_libraries(system_status_node system_status_parameters PkgConfig::CPPUPROFILE) -install(DIRECTORY include/ DESTINATION include/) -install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) - install(TARGETS system_status_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_system_status test/test_system_status_node.cpp src/system_status_node.cpp) - target_include_directories(${PROJECT_NAME}_test_system_status - PUBLIC ${CMAKE_INSTALL_PREFIX}/include) - + target_include_directories( + ${PROJECT_NAME}_test_system_status + PUBLIC $ + $ ${CMAKE_INSTALL_PREFIX}/include) ament_target_dependencies(${PROJECT_NAME}_test_system_status - ament_cmake_gtest ${PACKAGE_INCLUDE_DEPENDS}) + ${PACKAGE_DEPENDENCIES}) target_link_libraries(${PROJECT_NAME}_test_system_status system_status_parameters PkgConfig::CPPUPROFILE) diff --git a/panther_diagnostics/cmake/SuperBuild.cmake b/panther_diagnostics/cmake/SuperBuild.cmake index 1b2c25333..4370b4cdd 100644 --- a/panther_diagnostics/cmake/SuperBuild.cmake +++ b/panther_diagnostics/cmake/SuperBuild.cmake @@ -20,11 +20,11 @@ list(APPEND DEPENDENCIES ep_cppuprofile) ExternalProject_Add( ep_cppuprofile + INSTALL_DIR ${CMAKE_INSTALL_PREFIX} GIT_REPOSITORY https://github.com/Orange-OpenSource/cppuprofile.git GIT_TAG 1.1.1 PREFIX ${CMAKE_CURRENT_BINARY_DIR}/ep_cppuprofile BUILD_COMMAND $(MAKE) -C - INSTALL_DIR ${CMAKE_INSTALL_PREFIX} INSTALL_COMMAND make install INSTALL_PREFIX= CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} BUILD_IN_SOURCE 1) diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt index 5b53b717b..d0fa2e10c 100644 --- a/panther_gazebo/CMakeLists.txt +++ b/panther_gazebo/CMakeLists.txt @@ -5,12 +5,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(ignition-transport11 QUIET REQUIRED) -find_package(ignition-common4 REQUIRED) -find_package(ignition-msgs8 REQUIRED) -find_package(panther_utils REQUIRED) -find_package(yaml-cpp REQUIRED) +set(PACKAGE_DEPENDENCIES ament_cmake ignition-transport11 ignition-common4 + ignition-msgs8 panther_utils yaml-cpp) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() include_directories(include) @@ -20,13 +20,14 @@ set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) add_executable(gz_led_strip_manager src/main.cpp src/gz_led_strip_manager.cpp src/gz_led_strip.cpp) +ament_target_dependencies(gz_led_strip_manager panther_utils) target_link_libraries( gz_led_strip_manager ignition-transport${IGN_TRANSPORT_VER}::core ignition-msgs${IGN_MSGS_VER} ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} yaml-cpp) -ament_target_dependencies(gz_led_strip_manager panther_utils) install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt index 24e81fded..4243d8842 100644 --- a/panther_hardware_interfaces/CMakeLists.txt +++ b/panther_hardware_interfaces/CMakeLists.txt @@ -11,17 +11,11 @@ else() project(panther_hardware_interfaces) endif() -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# Find dependencies -set(PACKAGE_INCLUDE_DEPENDS +set(PACKAGE_DEPENDENCIES ament_cmake controller_interface diagnostic_updater @@ -41,10 +35,12 @@ set(PACKAGE_INCLUDE_DEPENDS tf2_geometry_msgs tf2_ros) -foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) endforeach() +include_directories(include) + generate_parameter_library(phidgets_spatial_parameters config/phidgets_spatial_parameters.yaml) @@ -68,11 +64,7 @@ add_library( src/roboteq_driver.cpp src/roboteq_error_filter.cpp src/utils.cpp) - -target_include_directories(${PROJECT_NAME} PRIVATE include) - -ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_INCLUDE_DEPENDS}) - +ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_DEPENDENCIES}) target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD phidgets_spatial_parameters) @@ -82,8 +74,6 @@ target_compile_definitions(${PROJECT_NAME} pluginlib_export_plugin_description_file(hardware_interface panther_hardware_interfaces.xml) -install(DIRECTORY include/ DESTINATION include) - install( TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin @@ -99,7 +89,8 @@ if(BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}_test_panther_imu test/test_panther_imu.cpp) target_include_directories( ${PROJECT_NAME}_test_panther_imu - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_panther_imu hardware_interface rclcpp panther_utils panther_msgs phidgets_api) target_link_libraries(${PROJECT_NAME}_test_panther_imu ${PROJECT_NAME} @@ -108,11 +99,16 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_roboteq_error_filter test/test_roboteq_error_filter.cpp src/roboteq_error_filter.cpp) - target_include_directories(${PROJECT_NAME}_test_roboteq_error_filter - PRIVATE include) + target_include_directories( + ${PROJECT_NAME}_test_roboteq_error_filter + PUBLIC $ + $) ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp src/utils.cpp) - target_include_directories(${PROJECT_NAME}_test_utils PRIVATE include) + target_include_directories( + ${PROJECT_NAME}_test_utils + PUBLIC $ + $) ament_add_gtest( ${PROJECT_NAME}_test_roboteq_data_converters @@ -120,7 +116,8 @@ if(BUILD_TESTING) src/utils.cpp) target_include_directories( ${PROJECT_NAME}_test_roboteq_data_converters - PRIVATE $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_roboteq_data_converters panther_msgs panther_utils) target_link_libraries(${PROJECT_NAME}_test_roboteq_data_converters @@ -132,7 +129,8 @@ if(BUILD_TESTING) src/roboteq_driver.cpp src/utils.cpp) target_include_directories( ${PROJECT_NAME}_test_canopen_controller - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_canopen_controller rclcpp panther_msgs panther_utils) target_link_libraries(${PROJECT_NAME}_test_canopen_controller @@ -144,7 +142,8 @@ if(BUILD_TESTING) src/roboteq_driver.cpp src/utils.cpp) target_include_directories( ${PROJECT_NAME}_test_roboteq_driver - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_roboteq_driver rclcpp panther_msgs panther_utils) target_link_libraries(${PROJECT_NAME}_test_roboteq_driver @@ -161,7 +160,8 @@ if(BUILD_TESTING) src/utils.cpp) target_include_directories( ${PROJECT_NAME}_test_motors_controller - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_motors_controller rclcpp panther_msgs panther_utils) target_link_libraries(${PROJECT_NAME}_test_motors_controller @@ -197,7 +197,8 @@ if(BUILD_TESTING) src/gpio_driver.cpp) target_include_directories( ${PROJECT_NAME}_test_panther_system_ros_interface - PRIVATE $ include) + PUBLIC $ + $) ament_target_dependencies( ${PROJECT_NAME}_test_panther_system_ros_interface diagnostic_updater @@ -216,7 +217,8 @@ if(BUILD_TESTING) 120) target_include_directories( ${PROJECT_NAME}_test_panther_system - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies( ${PROJECT_NAME}_test_panther_system hardware_interface rclcpp panther_msgs panther_utils) @@ -227,9 +229,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) - ament_export_libraries(${PROJECT_NAME}) - -ament_export_dependencies(${PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies(${PACKAGE_DEPENDENCIES}) ament_package() diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index f75480567..6231cf2da 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -5,17 +5,22 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(image_transport REQUIRED) -find_package(panther_msgs REQUIRED) -find_package(panther_utils REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(yaml-cpp REQUIRED) +set(PACKAGE_DEPENDENCIES + ament_cmake + diagnostic_updater + image_transport + panther_msgs + panther_utils + pluginlib + rclcpp + rclcpp_components + sensor_msgs + std_srvs + yaml-cpp) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() include_directories(include) @@ -81,14 +86,8 @@ install(TARGETS panther_animation_plugins LIBRARY DESTINATION lib) install(DIRECTORY animations config launch DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY include/ DESTINATION include) -ament_export_include_directories(include) - -ament_export_libraries(panther_animation_plugins) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - find_package(panther_utils REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_animation test/test_animation.cpp) target_include_directories( @@ -208,4 +207,7 @@ if(BUILD_TESTING) lights_driver_node_component) endif() +ament_export_include_directories(include) +ament_export_libraries(panther_animation_plugins) + ament_package() diff --git a/panther_localization/CMakeLists.txt b/panther_localization/CMakeLists.txt index 3dd97ab9d..33da28a6f 100644 --- a/panther_localization/CMakeLists.txt +++ b/panther_localization/CMakeLists.txt @@ -3,6 +3,6 @@ project(panther_localization) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index a10271219..05bc84e6e 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -5,7 +5,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_INCLUDE_DEPENDS +set(PACKAGE_DEPENDENCIES ament_cmake ament_index_cpp behaviortree_cpp @@ -19,8 +19,8 @@ set(PACKAGE_INCLUDE_DEPENDS std_srvs yaml-cpp) -foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) endforeach() include_directories(include) @@ -57,7 +57,7 @@ list(APPEND plugin_libs tick_after_timeout_bt_node) foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) - ament_target_dependencies(${bt_plugin} ${PACKAGE_INCLUDE_DEPENDS}) + ament_target_dependencies(${bt_plugin} ${PACKAGE_DEPENDENCIES}) endforeach() add_executable( @@ -99,9 +99,12 @@ if(BUILD_TESTING) add_library(${PROJECT_NAME}_test_plugin_utils test/plugins/src/plugin_test_utils.cpp) - target_include_directories(${PROJECT_NAME}_test_plugin_utils - PUBLIC test/plugins/include) - + target_include_directories( + ${PROJECT_NAME}_test_plugin_utils + PUBLIC $ + $ test/plugins/include) + ament_target_dependencies(${PROJECT_NAME}_test_plugin_utils + ${PACKAGE_DEPENDENCIES}) target_link_libraries( ${PROJECT_NAME}_test_plugin_utils call_set_bool_service_bt_node @@ -112,9 +115,6 @@ if(BUILD_TESTING) shutdown_hosts_from_file_bt_node tick_after_timeout_bt_node) - ament_target_dependencies(${PROJECT_NAME}_test_plugin_utils - ${PACKAGE_INCLUDE_DEPENDS}) - ament_add_gtest(${PROJECT_NAME}_test_call_set_bool_service_node test/plugins/test_call_set_bool_service_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_call_set_bool_service_node) @@ -154,7 +154,7 @@ if(BUILD_TESTING) foreach(bt_node_test ${plugin_tests}) target_link_libraries(${bt_node_test} ${PROJECT_NAME}_test_plugin_utils) - ament_target_dependencies(${bt_node_test} ${PACKAGE_INCLUDE_DEPENDS}) + ament_target_dependencies(${bt_node_test} ${PACKAGE_DEPENDENCIES}) endforeach() ament_add_gtest(${PROJECT_NAME}_test_behavior_tree_utils diff --git a/panther_utils/CMakeLists.txt b/panther_utils/CMakeLists.txt index efe62fca2..9fbca5440 100644 --- a/panther_utils/CMakeLists.txt +++ b/panther_utils/CMakeLists.txt @@ -5,22 +5,24 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(yaml-cpp REQUIRED) +set(PACKAGE_DEPENDENCIES + ament_cmake + ament_cmake_python + diagnostic_updater + realtime_tools + rclcpp + std_msgs + yaml-cpp) -include_directories(${GTEST_INCLUDE_DIRS}) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() install(DIRECTORY include/ DESTINATION include) -ament_export_include_directories(include) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(${PROJECT_NAME}_test_common_utilities test/test_common_utilities.cpp) target_include_directories( @@ -28,7 +30,6 @@ if(BUILD_TESTING) PUBLIC $ $) - find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_diagnostics test/test_diagnostics.cpp) target_include_directories( ${PROJECT_NAME}_test_diagnostics @@ -36,7 +37,6 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_diagnostics diagnostic_updater) - find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_moving_average test/test_moving_average.cpp) target_include_directories( @@ -73,8 +73,8 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_yaml_utils yaml-cpp) endif() +ament_export_include_directories(include) ament_export_dependencies(realtime_tools) - -ament_python_install_package(panther_utils) +ament_python_install_package(${PROJECT_NAME}) ament_package() From 5d210e9fc0588d6c9aea18e26c3fd7fb50c28a1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Irzyk?= <108666440+pawelirh@users.noreply.github.com> Date: Mon, 5 Aug 2024 14:54:39 +0200 Subject: [PATCH 06/67] Change scheme theme (#380) --- .docs/panther_ros2_api.drawio.svg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.docs/panther_ros2_api.drawio.svg b/.docs/panther_ros2_api.drawio.svg index cdee93061..473b1e497 100644 --- a/.docs/panther_ros2_api.drawio.svg +++ b/.docs/panther_ros2_api.drawio.svg @@ -1,4 +1,4 @@ -
panther_localization
panther_localization
panther_controller
panther_controller
panther_lights
panther_lights
panther_hardware_interfaces
panther_hardware_interfaces
panther_battery
panther_battery
panther_manager
panther_manager
panther_diagnostics
panther_diagnostics
PantherSystem
PantherSystem
hardware_controller
hardware_controller
GPIOController
GPIOController
MotorsController
MotorsController
CANopenController
CANopenController
RoboteqDriver
RoboteqDriver
CAN
CAN
EStopManager
EStopManager
battery_driver
battery_driver
RPi GPIO
RPi GPIO
ADC
ADC
imu_broadcaster
imu_broadcaster
joint_state_broadcaster
joint_state_broadcaster
drive_controller
drive_controller
controller_manager
controller_manager
PantherImuSensor

PantherImuSensor
IMU
IMU
ekf_filter
ekf_filter
ImuFilter
ImuFilter
Spatial
Spatial
imu/data
imu/data
odometry/wheels
odometry/wheels
odometry/filtered
odometry/filtered
joint_states
joint_states
cmd_vel
cmd_vel
dynamic_joint_states
dynamic_joint_states
 hardware/e_stop
 hardware/e_stop
hardware/motor_controllers_state
hardware/motor_controllers_state
 hardware/io_state
 hardware/io_state
hardware/e_stop_trigger
hardware/e_stop_trigger
hardware/e_stop_reset
hardware/e_stop_reset
hardware/<hw_component>_enable
hardware/<hw_component>_enable
robot_state_publisher
robot_state_publisher
robot_description
robot_description
/tf
/tf
/tf_static
/tf_static
/tf
/tf
battery/battery_status
battery/battery_status
Bumper Lights
Bumper Lights
lights_driver
lights_driver
lights_controller
lights_controller
lights/channel_1_frame
lights/channel_1_frame
lights/channel_2_frame
lights/channel_2_frame
lights/set_animation
lights/set_animation
lights/set_brightness
lights/set_brightness
safety_manager
safety_manager
system_monitor
system_monitor
system_status
system_status
lights_manager
lights_manager
hardware/led_control_enable
hardware/led_control_enable
/tf
/tf
battery/charging_status
battery/charging_status
GPIODriver
GPIODriver
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
Legend:
Legend:
Nodes
Nodes
Plugin
Plugin
Objects
Objects
Topics
Topics
Services
Services
Data Transfer
Data Trans...Text is not SVG - cannot display +
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 \ No newline at end of file From 247de54cef1c78d45282c52c2600e98b44d94a77 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 6 Aug 2024 08:43:00 +0200 Subject: [PATCH 07/67] Add dependencies --- panther_gazebo/package.xml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index 74026ba87..af1ab5412 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -17,12 +17,19 @@ ament_cmake + hardware_interface + ign_ros2_control panther_utils + pluginlib + rclcpp + rclcpp_lifecycle + sensor_msgs + std_msgs + std_srvs yaml-cpp controller_manager husarion_gz_worlds - ign_ros2_control launch launch_ros nav2_common From 6cf54ab06b9d0bb782b181f1c7acc0fc8ba5367b Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 6 Aug 2024 08:46:48 +0200 Subject: [PATCH 08/67] Add remapping --- panther_description/urdf/gazebo.urdf.xacro | 2 ++ 1 file changed, 2 insertions(+) diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro index 3884e7ee6..6eea09415 100644 --- a/panther_description/urdf/gazebo.urdf.xacro +++ b/panther_description/urdf/gazebo.urdf.xacro @@ -43,6 +43,8 @@ ${namespace} gz_ros2_control/e_stop:=hardware/e_stop + gz_ros2_control/e_stop_reset:=hardware/e_stop_reset + gz_ros2_control/e_stop_trigger:=hardware/e_stop_trigger imu_broadcaster/imu:=imu/data drive_controller/cmd_vel_unstamped:=cmd_vel drive_controller/odom:=odometry/wheels From de6d61aa51975e3a90a4b9a76fa339fa9795c257 Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Tue, 6 Aug 2024 15:09:01 +0200 Subject: [PATCH 09/67] unify CMakeLists.txt files (#381) --- panther_battery/CMakeLists.txt | 46 +++++++---------- panther_controller/CMakeLists.txt | 2 +- panther_description/CMakeLists.txt | 1 + panther_diagnostics/CMakeLists.txt | 36 +++++++------- panther_diagnostics/cmake/SuperBuild.cmake | 2 +- panther_gazebo/CMakeLists.txt | 15 +++--- panther_hardware_interfaces/CMakeLists.txt | 58 +++++++++++----------- panther_lights/CMakeLists.txt | 36 +++++++------- panther_localization/CMakeLists.txt | 2 +- panther_manager/CMakeLists.txt | 22 ++++---- panther_utils/CMakeLists.txt | 28 +++++------ 11 files changed, 122 insertions(+), 126 deletions(-) diff --git a/panther_battery/CMakeLists.txt b/panther_battery/CMakeLists.txt index 9bc58a551..37fd6c814 100644 --- a/panther_battery/CMakeLists.txt +++ b/panther_battery/CMakeLists.txt @@ -5,12 +5,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(panther_msgs REQUIRED) -find_package(panther_utils REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) +set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater panther_msgs + panther_utils rclcpp sensor_msgs) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() include_directories(include ${panther_utils_INCLUDE_DIRS}) @@ -23,9 +23,7 @@ add_executable( src/battery_publisher.cpp src/dual_battery_publisher.cpp src/single_battery_publisher.cpp) - -ament_target_dependencies(battery_node diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) +ament_target_dependencies(battery_node ${PACKAGE_DEPENDENCIES}) install(TARGETS battery_node DESTINATION lib/${PROJECT_NAME}) @@ -73,9 +71,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_publisher diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_single_battery_publisher @@ -85,9 +82,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_single_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_single_battery_publisher diagnostic_updater - panther_msgs panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_single_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_dual_battery_publisher @@ -97,9 +93,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_dual_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_dual_battery_publisher diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_dual_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_battery_node @@ -114,9 +109,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_node PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_node + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_battery_node_roboteq @@ -131,9 +125,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_node_roboteq PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node_roboteq diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_node_roboteq + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_battery_node_dual_bat @@ -148,9 +141,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_node_dual_bat PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node_dual_bat diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_node_dual_bat + ${PACKAGE_DEPENDENCIES}) endif() diff --git a/panther_controller/CMakeLists.txt b/panther_controller/CMakeLists.txt index b71172368..c6489681f 100644 --- a/panther_controller/CMakeLists.txt +++ b/panther_controller/CMakeLists.txt @@ -3,6 +3,6 @@ project(panther_controller) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_description/CMakeLists.txt b/panther_description/CMakeLists.txt index 741a0a391..b0c336d73 100644 --- a/panther_description/CMakeLists.txt +++ b/panther_description/CMakeLists.txt @@ -8,4 +8,5 @@ install(DIRECTORY config launch meshes rviz urdf ament_environment_hooks( "${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in") + ament_package() diff --git a/panther_diagnostics/CMakeLists.txt b/panther_diagnostics/CMakeLists.txt index 6c73bb2f4..c94d1c1e4 100644 --- a/panther_diagnostics/CMakeLists.txt +++ b/panther_diagnostics/CMakeLists.txt @@ -15,25 +15,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_INCLUDE_DEPENDS +set(PACKAGE_DEPENDENCIES ament_cmake - rclcpp - panther_msgs - panther_utils - diagnostic_updater diagnostic_msgs - std_msgs + diagnostic_updater generate_parameter_library - PkgConfig) + panther_msgs + panther_utils + PkgConfig + rclcpp + std_msgs) -foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) endforeach() include_directories(include) set(CPPUPROFILE_PREFIX ${CMAKE_BINARY_DIR}/ep_cppuprofile/src/ep_cppuprofile) set(ENV{PKG_CONFIG_PATH} "${CPPUPROFILE_PREFIX}/lib:$ENV{PKG_CONFIG_PATH}") + pkg_check_modules(CPPUPROFILE REQUIRED IMPORTED_TARGET cppuprofile) generate_parameter_library(system_status_parameters @@ -42,26 +43,25 @@ generate_parameter_library(system_status_parameters add_executable(system_status_node src/main.cpp src/system_status_node.cpp) target_include_directories(system_status_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include) - -ament_target_dependencies(system_status_node ${PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(system_status_node ${PACKAGE_DEPENDENCIES}) target_link_libraries(system_status_node system_status_parameters PkgConfig::CPPUPROFILE) -install(DIRECTORY include/ DESTINATION include/) -install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) - install(TARGETS system_status_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_system_status test/test_system_status_node.cpp src/system_status_node.cpp) - target_include_directories(${PROJECT_NAME}_test_system_status - PUBLIC ${CMAKE_INSTALL_PREFIX}/include) - + target_include_directories( + ${PROJECT_NAME}_test_system_status + PUBLIC $ + $ ${CMAKE_INSTALL_PREFIX}/include) ament_target_dependencies(${PROJECT_NAME}_test_system_status - ament_cmake_gtest ${PACKAGE_INCLUDE_DEPENDS}) + ${PACKAGE_DEPENDENCIES}) target_link_libraries(${PROJECT_NAME}_test_system_status system_status_parameters PkgConfig::CPPUPROFILE) diff --git a/panther_diagnostics/cmake/SuperBuild.cmake b/panther_diagnostics/cmake/SuperBuild.cmake index 1b2c25333..4370b4cdd 100644 --- a/panther_diagnostics/cmake/SuperBuild.cmake +++ b/panther_diagnostics/cmake/SuperBuild.cmake @@ -20,11 +20,11 @@ list(APPEND DEPENDENCIES ep_cppuprofile) ExternalProject_Add( ep_cppuprofile + INSTALL_DIR ${CMAKE_INSTALL_PREFIX} GIT_REPOSITORY https://github.com/Orange-OpenSource/cppuprofile.git GIT_TAG 1.1.1 PREFIX ${CMAKE_CURRENT_BINARY_DIR}/ep_cppuprofile BUILD_COMMAND $(MAKE) -C - INSTALL_DIR ${CMAKE_INSTALL_PREFIX} INSTALL_COMMAND make install INSTALL_PREFIX= CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} BUILD_IN_SOURCE 1) diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt index 5b53b717b..d0fa2e10c 100644 --- a/panther_gazebo/CMakeLists.txt +++ b/panther_gazebo/CMakeLists.txt @@ -5,12 +5,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(ignition-transport11 QUIET REQUIRED) -find_package(ignition-common4 REQUIRED) -find_package(ignition-msgs8 REQUIRED) -find_package(panther_utils REQUIRED) -find_package(yaml-cpp REQUIRED) +set(PACKAGE_DEPENDENCIES ament_cmake ignition-transport11 ignition-common4 + ignition-msgs8 panther_utils yaml-cpp) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() include_directories(include) @@ -20,13 +20,14 @@ set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) add_executable(gz_led_strip_manager src/main.cpp src/gz_led_strip_manager.cpp src/gz_led_strip.cpp) +ament_target_dependencies(gz_led_strip_manager panther_utils) target_link_libraries( gz_led_strip_manager ignition-transport${IGN_TRANSPORT_VER}::core ignition-msgs${IGN_MSGS_VER} ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} yaml-cpp) -ament_target_dependencies(gz_led_strip_manager panther_utils) install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt index 24e81fded..4243d8842 100644 --- a/panther_hardware_interfaces/CMakeLists.txt +++ b/panther_hardware_interfaces/CMakeLists.txt @@ -11,17 +11,11 @@ else() project(panther_hardware_interfaces) endif() -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# Find dependencies -set(PACKAGE_INCLUDE_DEPENDS +set(PACKAGE_DEPENDENCIES ament_cmake controller_interface diagnostic_updater @@ -41,10 +35,12 @@ set(PACKAGE_INCLUDE_DEPENDS tf2_geometry_msgs tf2_ros) -foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) endforeach() +include_directories(include) + generate_parameter_library(phidgets_spatial_parameters config/phidgets_spatial_parameters.yaml) @@ -68,11 +64,7 @@ add_library( src/roboteq_driver.cpp src/roboteq_error_filter.cpp src/utils.cpp) - -target_include_directories(${PROJECT_NAME} PRIVATE include) - -ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_INCLUDE_DEPENDS}) - +ament_target_dependencies(${PROJECT_NAME} ${PACKAGE_DEPENDENCIES}) target_link_libraries(${PROJECT_NAME} PkgConfig::LIBLELY_COAPP PkgConfig::LIBGPIOD phidgets_spatial_parameters) @@ -82,8 +74,6 @@ target_compile_definitions(${PROJECT_NAME} pluginlib_export_plugin_description_file(hardware_interface panther_hardware_interfaces.xml) -install(DIRECTORY include/ DESTINATION include) - install( TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin @@ -99,7 +89,8 @@ if(BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}_test_panther_imu test/test_panther_imu.cpp) target_include_directories( ${PROJECT_NAME}_test_panther_imu - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_panther_imu hardware_interface rclcpp panther_utils panther_msgs phidgets_api) target_link_libraries(${PROJECT_NAME}_test_panther_imu ${PROJECT_NAME} @@ -108,11 +99,16 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_roboteq_error_filter test/test_roboteq_error_filter.cpp src/roboteq_error_filter.cpp) - target_include_directories(${PROJECT_NAME}_test_roboteq_error_filter - PRIVATE include) + target_include_directories( + ${PROJECT_NAME}_test_roboteq_error_filter + PUBLIC $ + $) ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp src/utils.cpp) - target_include_directories(${PROJECT_NAME}_test_utils PRIVATE include) + target_include_directories( + ${PROJECT_NAME}_test_utils + PUBLIC $ + $) ament_add_gtest( ${PROJECT_NAME}_test_roboteq_data_converters @@ -120,7 +116,8 @@ if(BUILD_TESTING) src/utils.cpp) target_include_directories( ${PROJECT_NAME}_test_roboteq_data_converters - PRIVATE $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_roboteq_data_converters panther_msgs panther_utils) target_link_libraries(${PROJECT_NAME}_test_roboteq_data_converters @@ -132,7 +129,8 @@ if(BUILD_TESTING) src/roboteq_driver.cpp src/utils.cpp) target_include_directories( ${PROJECT_NAME}_test_canopen_controller - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_canopen_controller rclcpp panther_msgs panther_utils) target_link_libraries(${PROJECT_NAME}_test_canopen_controller @@ -144,7 +142,8 @@ if(BUILD_TESTING) src/roboteq_driver.cpp src/utils.cpp) target_include_directories( ${PROJECT_NAME}_test_roboteq_driver - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_roboteq_driver rclcpp panther_msgs panther_utils) target_link_libraries(${PROJECT_NAME}_test_roboteq_driver @@ -161,7 +160,8 @@ if(BUILD_TESTING) src/utils.cpp) target_include_directories( ${PROJECT_NAME}_test_motors_controller - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME}_test_motors_controller rclcpp panther_msgs panther_utils) target_link_libraries(${PROJECT_NAME}_test_motors_controller @@ -197,7 +197,8 @@ if(BUILD_TESTING) src/gpio_driver.cpp) target_include_directories( ${PROJECT_NAME}_test_panther_system_ros_interface - PRIVATE $ include) + PUBLIC $ + $) ament_target_dependencies( ${PROJECT_NAME}_test_panther_system_ros_interface diagnostic_updater @@ -216,7 +217,8 @@ if(BUILD_TESTING) 120) target_include_directories( ${PROJECT_NAME}_test_panther_system - PUBLIC $ include) + PUBLIC $ + $) ament_target_dependencies( ${PROJECT_NAME}_test_panther_system hardware_interface rclcpp panther_msgs panther_utils) @@ -227,9 +229,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) - ament_export_libraries(${PROJECT_NAME}) - -ament_export_dependencies(${PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies(${PACKAGE_DEPENDENCIES}) ament_package() diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index f75480567..6231cf2da 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -5,17 +5,22 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(image_transport REQUIRED) -find_package(panther_msgs REQUIRED) -find_package(panther_utils REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(yaml-cpp REQUIRED) +set(PACKAGE_DEPENDENCIES + ament_cmake + diagnostic_updater + image_transport + panther_msgs + panther_utils + pluginlib + rclcpp + rclcpp_components + sensor_msgs + std_srvs + yaml-cpp) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() include_directories(include) @@ -81,14 +86,8 @@ install(TARGETS panther_animation_plugins LIBRARY DESTINATION lib) install(DIRECTORY animations config launch DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY include/ DESTINATION include) -ament_export_include_directories(include) - -ament_export_libraries(panther_animation_plugins) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - find_package(panther_utils REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_animation test/test_animation.cpp) target_include_directories( @@ -208,4 +207,7 @@ if(BUILD_TESTING) lights_driver_node_component) endif() +ament_export_include_directories(include) +ament_export_libraries(panther_animation_plugins) + ament_package() diff --git a/panther_localization/CMakeLists.txt b/panther_localization/CMakeLists.txt index 3dd97ab9d..33da28a6f 100644 --- a/panther_localization/CMakeLists.txt +++ b/panther_localization/CMakeLists.txt @@ -3,6 +3,6 @@ project(panther_localization) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index a10271219..05bc84e6e 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -5,7 +5,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_INCLUDE_DEPENDS +set(PACKAGE_DEPENDENCIES ament_cmake ament_index_cpp behaviortree_cpp @@ -19,8 +19,8 @@ set(PACKAGE_INCLUDE_DEPENDS std_srvs yaml-cpp) -foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) endforeach() include_directories(include) @@ -57,7 +57,7 @@ list(APPEND plugin_libs tick_after_timeout_bt_node) foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) - ament_target_dependencies(${bt_plugin} ${PACKAGE_INCLUDE_DEPENDS}) + ament_target_dependencies(${bt_plugin} ${PACKAGE_DEPENDENCIES}) endforeach() add_executable( @@ -99,9 +99,12 @@ if(BUILD_TESTING) add_library(${PROJECT_NAME}_test_plugin_utils test/plugins/src/plugin_test_utils.cpp) - target_include_directories(${PROJECT_NAME}_test_plugin_utils - PUBLIC test/plugins/include) - + target_include_directories( + ${PROJECT_NAME}_test_plugin_utils + PUBLIC $ + $ test/plugins/include) + ament_target_dependencies(${PROJECT_NAME}_test_plugin_utils + ${PACKAGE_DEPENDENCIES}) target_link_libraries( ${PROJECT_NAME}_test_plugin_utils call_set_bool_service_bt_node @@ -112,9 +115,6 @@ if(BUILD_TESTING) shutdown_hosts_from_file_bt_node tick_after_timeout_bt_node) - ament_target_dependencies(${PROJECT_NAME}_test_plugin_utils - ${PACKAGE_INCLUDE_DEPENDS}) - ament_add_gtest(${PROJECT_NAME}_test_call_set_bool_service_node test/plugins/test_call_set_bool_service_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_call_set_bool_service_node) @@ -154,7 +154,7 @@ if(BUILD_TESTING) foreach(bt_node_test ${plugin_tests}) target_link_libraries(${bt_node_test} ${PROJECT_NAME}_test_plugin_utils) - ament_target_dependencies(${bt_node_test} ${PACKAGE_INCLUDE_DEPENDS}) + ament_target_dependencies(${bt_node_test} ${PACKAGE_DEPENDENCIES}) endforeach() ament_add_gtest(${PROJECT_NAME}_test_behavior_tree_utils diff --git a/panther_utils/CMakeLists.txt b/panther_utils/CMakeLists.txt index efe62fca2..9fbca5440 100644 --- a/panther_utils/CMakeLists.txt +++ b/panther_utils/CMakeLists.txt @@ -5,22 +5,24 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(yaml-cpp REQUIRED) +set(PACKAGE_DEPENDENCIES + ament_cmake + ament_cmake_python + diagnostic_updater + realtime_tools + rclcpp + std_msgs + yaml-cpp) -include_directories(${GTEST_INCLUDE_DIRS}) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() install(DIRECTORY include/ DESTINATION include) -ament_export_include_directories(include) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(${PROJECT_NAME}_test_common_utilities test/test_common_utilities.cpp) target_include_directories( @@ -28,7 +30,6 @@ if(BUILD_TESTING) PUBLIC $ $) - find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_diagnostics test/test_diagnostics.cpp) target_include_directories( ${PROJECT_NAME}_test_diagnostics @@ -36,7 +37,6 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_diagnostics diagnostic_updater) - find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_moving_average test/test_moving_average.cpp) target_include_directories( @@ -73,8 +73,8 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_yaml_utils yaml-cpp) endif() +ament_export_include_directories(include) ament_export_dependencies(realtime_tools) - -ament_python_install_package(panther_utils) +ament_python_install_package(${PROJECT_NAME}) ament_package() From b7ad8a36d087cbe2257baf01643e0bc515dde338 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 6 Aug 2024 13:32:17 +0000 Subject: [PATCH 10/67] Rename files in panther_diagnostics package --- .docs/panther_ros2_api.drawio.svg | 2 +- panther_diagnostics/CMakeLists.txt | 25 ++++----- panther_diagnostics/README.md | 14 ++--- ...us_parameters.yaml => system_monitor.yaml} | 10 ++-- ...tatus_node.hpp => system_monitor_node.hpp} | 16 +++--- ...tus.launch.py => system_monitor.launch.py} | 6 +- panther_diagnostics/src/main.cpp | 12 ++-- ...tatus_node.cpp => system_monitor_node.cpp} | 39 +++++++------ ..._node.cpp => test_system_monitor_node.cpp} | 56 +++++++++---------- 9 files changed, 91 insertions(+), 89 deletions(-) rename panther_diagnostics/config/{system_status_parameters.yaml => system_monitor.yaml} (87%) rename panther_diagnostics/include/panther_diagnostics/{system_status_node.hpp => system_monitor_node.hpp} (81%) rename panther_diagnostics/launch/{system_status.launch.py => system_monitor.launch.py} (93%) rename panther_diagnostics/src/{system_status_node.cpp => system_monitor_node.cpp} (79%) rename panther_diagnostics/test/{test_system_status_node.cpp => test_system_monitor_node.cpp} (60%) diff --git a/.docs/panther_ros2_api.drawio.svg b/.docs/panther_ros2_api.drawio.svg index 473b1e497..29cefa8b9 100644 --- a/.docs/panther_ros2_api.drawio.svg +++ b/.docs/panther_ros2_api.drawio.svg @@ -1,4 +1,4 @@ -
panther_localization
panther_localization
panther_controller
panther_controller
panther_lights
panther_lights
panther_hardware_interfaces
panther_hardware_interfaces
panther_battery
panther_battery
panther_manager
panther_manager
panther_diagnostics
panther_diagnostics
PantherSystem
PantherSystem
hardware_controller
hardware_controller
GPIOController
GPIOController
MotorsController
MotorsController
CANopenController
CANopenController
RoboteqDriver
RoboteqDriver
CAN
CAN
EStopManager
EStopManager
battery_driver
battery_driver
RPi GPIO
RPi GPIO
ADC
ADC
imu_broadcaster
imu_broadcaster
joint_state_broadcaster
joint_state_broadcaster
drive_controller
drive_controller
controller_manager
controller_manager
PantherImuSensor

PantherImuSensor
IMU
IMU
ekf_filter
ekf_filter
ImuFilter
ImuFilter
Spatial
Spatial
imu/data
imu/data
odometry/wheels
odometry/wheels
odometry/filtered
odometry/filtered
joint_states
joint_states
cmd_vel
cmd_vel
dynamic_joint_states
dynamic_joint_states
 hardware/e_stop
 hardware/e_stop
hardware/motor_controllers_state
hardware/motor_controllers_state
 hardware/io_state
 hardware/io_state
hardware/e_stop_trigger
hardware/e_stop_trigger
hardware/e_stop_reset
hardware/e_stop_reset
hardware/<hw_component>_enable
hardware/<hw_component>_enable
robot_state_publisher
robot_state_publisher
robot_description
robot_description
/tf
/tf
/tf_static
/tf_static
/tf
/tf
battery/battery_status
battery/battery_status
Bumper Lights
Bumper Lights
lights_driver
lights_driver
lights_controller
lights_controller
lights/channel_1_frame
lights/channel_1_frame
lights/channel_2_frame
lights/channel_2_frame
lights/set_animation
lights/set_animation
lights/set_brightness
lights/set_brightness
safety_manager
safety_manager
system_monitor
system_monitor
system_status
system_status
lights_manager
lights_manager
hardware/led_control_enable
hardware/led_control_enable
/tf
/tf
battery/charging_status
battery/charging_status
GPIODriver
GPIODriver
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
Legend:
Legend:
Nodes
Nodes
Plugin
Plugin
Objects
Objects
Topics
Topics
Services
Services
Data Transfer
Data Trans...Text is not SVG - cannot display \ No newline at end of file +
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/panther_diagnostics/CMakeLists.txt b/panther_diagnostics/CMakeLists.txt index c94d1c1e4..0a765d6b2 100644 --- a/panther_diagnostics/CMakeLists.txt +++ b/panther_diagnostics/CMakeLists.txt @@ -37,33 +37,32 @@ set(ENV{PKG_CONFIG_PATH} "${CPPUPROFILE_PREFIX}/lib:$ENV{PKG_CONFIG_PATH}") pkg_check_modules(CPPUPROFILE REQUIRED IMPORTED_TARGET cppuprofile) -generate_parameter_library(system_status_parameters - config/system_status_parameters.yaml) +generate_parameter_library(system_monitor_parameters config/system_monitor.yaml) -add_executable(system_status_node src/main.cpp src/system_status_node.cpp) -target_include_directories(system_status_node +add_executable(system_monitor_node src/main.cpp src/system_monitor_node.cpp) +target_include_directories(system_monitor_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include) -ament_target_dependencies(system_status_node ${PACKAGE_DEPENDENCIES}) -target_link_libraries(system_status_node system_status_parameters +ament_target_dependencies(system_monitor_node ${PACKAGE_DEPENDENCIES}) +target_link_libraries(system_monitor_node system_monitor_parameters PkgConfig::CPPUPROFILE) -install(TARGETS system_status_node DESTINATION lib/${PROJECT_NAME}) +install(TARGETS system_monitor_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(${PROJECT_NAME}_test_system_status - test/test_system_status_node.cpp src/system_status_node.cpp) + ament_add_gtest(${PROJECT_NAME}_test_system_monitor + test/test_system_monitor_node.cpp src/system_monitor_node.cpp) target_include_directories( - ${PROJECT_NAME}_test_system_status + ${PROJECT_NAME}_test_system_monitor PUBLIC $ $ ${CMAKE_INSTALL_PREFIX}/include) - ament_target_dependencies(${PROJECT_NAME}_test_system_status + ament_target_dependencies(${PROJECT_NAME}_test_system_monitor ${PACKAGE_DEPENDENCIES}) - target_link_libraries(${PROJECT_NAME}_test_system_status - system_status_parameters PkgConfig::CPPUPROFILE) + target_link_libraries(${PROJECT_NAME}_test_system_monitor + system_monitor_parameters PkgConfig::CPPUPROFILE) endif() diff --git a/panther_diagnostics/README.md b/panther_diagnostics/README.md index e4d412464..01f5f3355 100644 --- a/panther_diagnostics/README.md +++ b/panther_diagnostics/README.md @@ -4,27 +4,27 @@ Package containing nodes monitoring and publishing the Built-in Computer status ## Launch Files -- `system_status.launch.py`: Launch a node that analyzes the state of the most important components in the robot +- `system_monitor.launch.py`: Launch a node that analyzes the state of the most important components in the robot ## Configuration Files -- [`system_status_parameters.yaml`](./config/system_status_parameters.yaml): Defines parameters for `system_status_node`. +- [`system_monitor.yaml`](./config/system_monitor.yaml): Defines parameters for `system_monitor_node`. ## ROS Nodes -- [`system_status_node`](#system_status_node): Publishes system state of the Built-in Computer such as CPU usage, RAM usage, disk usage and CPU temperature. +### system_monitor_node -### system_status_node +Publishes the built-in computer system status , monitoring parameters as such as CPU usage, RAM usage, disk usage, and CPU temperature. #### Publishes -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System status diagnostic messages. -- `system_status` [*panther_msgs/SystemStatus*]: State of the system, including Built-in Computer's CPU temperature and load. +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System monitor diagnostic messages. +- `system_status` [*panther_msgs/SystemStatus*]: Built-in computer system status, includes the most important computation-related parameters. #### Parameters - `~frame_id` [*string*, default: **built_in_computer**]: Frame where computer is located. -- `~publish_rate` [*double*, default: **0.25**]: System status publish rate in seconds. +- `~publish_frequency` [*double*, default: **5.0**]: System status publishing frequency [Hz]. - `~disk_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for disk usage warning in percentage. - `~memory_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for memory usage warning in percentage. - `~cpu_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for CPU usage warning in percentage. diff --git a/panther_diagnostics/config/system_status_parameters.yaml b/panther_diagnostics/config/system_monitor.yaml similarity index 87% rename from panther_diagnostics/config/system_status_parameters.yaml rename to panther_diagnostics/config/system_monitor.yaml index 0fddc5f4e..18345bd2c 100644 --- a/panther_diagnostics/config/system_status_parameters.yaml +++ b/panther_diagnostics/config/system_monitor.yaml @@ -1,4 +1,4 @@ -system_status: +system_monitor: frame_id: type: string default_value: built_in_computer @@ -7,12 +7,12 @@ system_status: type: string default_value: Built-in Computer description: Name of hardware used in diagnostics. - publish_rate: + publish_frequency: type: double - default_value: 0.25 - description: System status publish rate in seconds. + default_value: 5.0 + description: System status publishing frequency [Hz]. validation: { - bounds<>: [0.0, 1000.0] + bounds<>: [0.1, 10.0] } disk_usage_warn_threshold: type: double diff --git a/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp b/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp similarity index 81% rename from panther_diagnostics/include/panther_diagnostics/system_status_node.hpp rename to panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp index 235d5d417..6292165b1 100644 --- a/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp +++ b/panther_diagnostics/include/panther_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_STATUS_NODE_HPP_ -#define PANTHER_DIAGNOSTICS_SYSTEM_STATUS_NODE_HPP_ +#ifndef PANTHER_DIAGNOSTICS__SYSTEM_MONITOR_NODE_HPP_ +#define PANTHER_DIAGNOSTICS__SYSTEM_MONITOR_NODE_HPP_ #include @@ -22,17 +22,17 @@ #include "panther_msgs/msg/system_status.hpp" -#include "system_status_parameters.hpp" +#include "system_monitor_parameters.hpp" using namespace std::chrono_literals; namespace panther_diagnostics { -class SystemStatusNode : public rclcpp::Node +class SystemMonitorNode : public rclcpp::Node { public: - SystemStatusNode(const std::string & node_name); + SystemMonitorNode(const std::string & node_name); struct SystemStatus { @@ -61,10 +61,10 @@ class SystemStatusNode : public rclcpp::Node rclcpp::Publisher::SharedPtr system_status_publisher_; diagnostic_updater::Updater diagnostic_updater_; - system_status::Params params_; - std::shared_ptr param_listener_; + system_monitor::Params params_; + std::shared_ptr param_listener_; static constexpr char kTemperatureInfoFilename[] = "/sys/class/thermal/thermal_zone0/temp"; }; } // namespace panther_diagnostics -#endif // PANTHER_DIAGNOSTICS_SYSTEM_STATUS_NODE_HPP_ +#endif // PANTHER_DIAGNOSTICS__SYSTEM_MONITOR_NODE_HPP_ diff --git a/panther_diagnostics/launch/system_status.launch.py b/panther_diagnostics/launch/system_monitor.launch.py similarity index 93% rename from panther_diagnostics/launch/system_status.launch.py rename to panther_diagnostics/launch/system_monitor.launch.py index 103acf598..e2d2ab706 100644 --- a/panther_diagnostics/launch/system_status.launch.py +++ b/panther_diagnostics/launch/system_monitor.launch.py @@ -28,9 +28,9 @@ def generate_launch_description(): description="Add namespace to all launched nodes", ) - system_status_node = Node( + system_monitor_node = Node( package="panther_diagnostics", - executable="system_status_node", + executable="system_monitor_node", name="system_monitor", namespace=namespace, remappings=[("/diagnostics", "diagnostics")], @@ -39,7 +39,7 @@ def generate_launch_description(): actions = [ declare_namespace_arg, - system_status_node, + system_monitor_node, ] return LaunchDescription(actions) diff --git a/panther_diagnostics/src/main.cpp b/panther_diagnostics/src/main.cpp index 69059d00c..7028ff212 100644 --- a/panther_diagnostics/src/main.cpp +++ b/panther_diagnostics/src/main.cpp @@ -18,23 +18,23 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_diagnostics/system_status_node.hpp" +#include "panther_diagnostics/system_monitor_node.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto system_status_node = - std::make_shared("system_monitor"); + auto system_monitor_node = + std::make_shared("system_monitor"); try { - rclcpp::spin(system_status_node); + rclcpp::spin(system_monitor_node); } catch (const std::runtime_error & e) { - std::cerr << "[" << system_status_node->get_name() << "] Caught exception: " << e.what() + std::cerr << "[" << system_monitor_node->get_name() << "] Caught exception: " << e.what() << std::endl; } - std::cout << "[" << system_status_node->get_name() << "] Shutting down" << std::endl; + std::cout << "[" << system_monitor_node->get_name() << "] Shutting down" << std::endl; rclcpp::shutdown(); return 0; } diff --git a/panther_diagnostics/src/system_status_node.cpp b/panther_diagnostics/src/system_monitor_node.cpp similarity index 79% rename from panther_diagnostics/src/system_status_node.cpp rename to panther_diagnostics/src/system_monitor_node.cpp index 51d1663f8..9007627a4 100644 --- a/panther_diagnostics/src/system_status_node.cpp +++ b/panther_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_status_node.hpp" +#include "panther_diagnostics/system_monitor_node.hpp" #include #include @@ -20,39 +20,42 @@ #include #include "cppuprofile/uprofile.h" - #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" #include "panther_msgs/msg/system_status.hpp" + #include "panther_utils/common_utilities.hpp" #include "panther_utils/ros_utils.hpp" namespace panther_diagnostics { -SystemStatusNode::SystemStatusNode(const std::string & node_name) +SystemMonitorNode::SystemMonitorNode(const std::string & node_name) : rclcpp::Node(node_name), diagnostic_updater_(this) { RCLCPP_INFO(this->get_logger(), "Initializing."); param_listener_ = - std::make_shared(this->get_node_parameters_interface()); + std::make_shared(this->get_node_parameters_interface()); params_ = param_listener_->get_params(); system_status_publisher_ = this->create_publisher( "system_status", 10); + + auto timer_interval_ms = static_cast(1000.0 / params_.publish_frequency); + timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(params_.publish_rate * 1000)), - std::bind(&SystemStatusNode::TimerCallback, this)); + std::chrono::milliseconds(timer_interval_ms), + std::bind(&SystemMonitorNode::TimerCallback, this)); diagnostic_updater_.setHardwareID(params_.hardware_id); - diagnostic_updater_.add("OS status", this, &SystemStatusNode::DiagnoseSystem); + diagnostic_updater_.add("OS status", this, &SystemMonitorNode::DiagnoseSystem); RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void SystemStatusNode::TimerCallback() +void SystemMonitorNode::TimerCallback() { const auto status = GetSystemStatus(); auto message = SystemStatusToMessage(status); @@ -60,9 +63,9 @@ void SystemStatusNode::TimerCallback() system_status_publisher_->publish(message); } -SystemStatusNode::SystemStatus SystemStatusNode::GetSystemStatus() const +SystemMonitorNode::SystemStatus SystemMonitorNode::GetSystemStatus() const { - SystemStatusNode::SystemStatus status; + SystemMonitorNode::SystemStatus status; status.core_usages = GetCoresUsages(); status.cpu_mean_usage = GetCPUMeanUsage(status.core_usages); @@ -73,19 +76,19 @@ SystemStatusNode::SystemStatus SystemStatusNode::GetSystemStatus() const return status; } -std::vector SystemStatusNode::GetCoresUsages() const +std::vector SystemMonitorNode::GetCoresUsages() const { std::vector loads = uprofile::getInstantCpuUsage(); return loads; } -float SystemStatusNode::GetCPUMeanUsage(const std::vector & usages) const +float SystemMonitorNode::GetCPUMeanUsage(const std::vector & usages) const { auto sum = std::accumulate(usages.begin(), usages.end(), 0.0); return sum / usages.size(); } -float SystemStatusNode::GetCPUTemperature(const std::string & filename) const +float SystemMonitorNode::GetCPUTemperature(const std::string & filename) const { try { auto file = panther_utils::common_utilities::OpenFile(filename, std::ios_base::in); @@ -100,14 +103,14 @@ float SystemStatusNode::GetCPUTemperature(const std::string & filename) const return std::numeric_limits::quiet_NaN(); } -float SystemStatusNode::GetMemoryUsage() const +float SystemMonitorNode::GetMemoryUsage() const { int total = 0, free = 0, available = 0; uprofile::getSystemMemory(total, free, available); return static_cast(total - available) / total * 100.0; } -float SystemStatusNode::GetDiskUsage() const +float SystemMonitorNode::GetDiskUsage() const { const std::filesystem::directory_entry entry("/"); const std::filesystem::space_info si = std::filesystem::space(entry.path()); @@ -115,8 +118,8 @@ float SystemStatusNode::GetDiskUsage() const return static_cast(si.capacity - si.available) / si.capacity * 100.0; } -panther_msgs::msg::SystemStatus SystemStatusNode::SystemStatusToMessage( - const SystemStatusNode::SystemStatus & status) +panther_msgs::msg::SystemStatus SystemMonitorNode::SystemStatusToMessage( + const SystemMonitorNode::SystemStatus & status) { panther_msgs::msg::SystemStatus message; @@ -132,7 +135,7 @@ panther_msgs::msg::SystemStatus SystemStatusNode::SystemStatusToMessage( return message; } -void SystemStatusNode::DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapper & status) +void SystemMonitorNode::DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapper & status) { auto error_level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string message = "System parameters are within acceptable limits."; diff --git a/panther_diagnostics/test/test_system_status_node.cpp b/panther_diagnostics/test/test_system_monitor_node.cpp similarity index 60% rename from panther_diagnostics/test/test_system_status_node.cpp rename to panther_diagnostics/test/test_system_monitor_node.cpp index 9f606cc55..dfd091911 100644 --- a/panther_diagnostics/test/test_system_status_node.cpp +++ b/panther_diagnostics/test/test_system_monitor_node.cpp @@ -19,71 +19,71 @@ #include "gtest/gtest.h" -#include "panther_diagnostics/system_status_node.hpp" +#include "panther_diagnostics/system_monitor_node.hpp" -class SystemStatusNodeWrapper : public panther_diagnostics::SystemStatusNode +class SystemMonitorNodeWrapper : public panther_diagnostics::SystemMonitorNode { public: - SystemStatusNodeWrapper() : panther_diagnostics::SystemStatusNode("test_system_statics") {} + SystemMonitorNodeWrapper() : panther_diagnostics::SystemMonitorNode("test_system_statics") {} std::vector GetCoresUsages() const { - return panther_diagnostics::SystemStatusNode::GetCoresUsages(); + return panther_diagnostics::SystemMonitorNode::GetCoresUsages(); } float GetCPUMeanUsage(const std::vector & usages) const { - return panther_diagnostics::SystemStatusNode::GetCPUMeanUsage(usages); + return panther_diagnostics::SystemMonitorNode::GetCPUMeanUsage(usages); } float GetCPUTemperature(const std::string & filename) const { - return panther_diagnostics::SystemStatusNode::GetCPUTemperature(filename); + return panther_diagnostics::SystemMonitorNode::GetCPUTemperature(filename); } - float GetMemoryUsage() const { return panther_diagnostics::SystemStatusNode::GetMemoryUsage(); } + float GetMemoryUsage() const { return panther_diagnostics::SystemMonitorNode::GetMemoryUsage(); } - float GetDiskUsage() const { return panther_diagnostics::SystemStatusNode::GetDiskUsage(); } + float GetDiskUsage() const { return panther_diagnostics::SystemMonitorNode::GetDiskUsage(); } panther_msgs::msg::SystemStatus SystemStatusToMessage( - const panther_diagnostics::SystemStatusNode::SystemStatus & status) + const panther_diagnostics::SystemMonitorNode::SystemStatus & status) { - return panther_diagnostics::SystemStatusNode::SystemStatusToMessage(status); + return panther_diagnostics::SystemMonitorNode::SystemStatusToMessage(status); } }; -class TestSystemStatusNode : public testing::Test +class TestSystemMonitorNode : public testing::Test { public: - TestSystemStatusNode(); + TestSystemMonitorNode(); protected: - std::unique_ptr system_status_; + std::unique_ptr system_monitor_; }; -TestSystemStatusNode::TestSystemStatusNode() +TestSystemMonitorNode::TestSystemMonitorNode() { - system_status_ = std::make_unique(); + system_monitor_ = std::make_unique(); } -TEST_F(TestSystemStatusNode, CheckCoresUsages) +TEST_F(TestSystemMonitorNode, CheckCoresUsages) { - const auto usages = system_status_->GetCoresUsages(); + const auto usages = system_monitor_->GetCoresUsages(); for (const auto & usage : usages) { EXPECT_TRUE((usage >= 0.0) && (usage <= 100.0)); } } -TEST_F(TestSystemStatusNode, CheckCPUMeanUsage) +TEST_F(TestSystemMonitorNode, CheckCPUMeanUsage) { std::vector usages = {45.0, 55.0, 45.0, 55.0}; - const auto mean = system_status_->GetCPUMeanUsage(usages); + const auto mean = system_monitor_->GetCPUMeanUsage(usages); EXPECT_FLOAT_EQ(mean, 50.0); } -TEST_F(TestSystemStatusNode, CheckTemperatureReadings) +TEST_F(TestSystemMonitorNode, CheckTemperatureReadings) { const std::string temperature_file_name = testing::TempDir() + "panther_diagnostics_temperature"; @@ -94,36 +94,36 @@ TEST_F(TestSystemStatusNode, CheckTemperatureReadings) temperature_file << 36600 << std::endl; temperature_file.close(); - const auto temperature = system_status_->GetCPUTemperature(temperature_file_name); + const auto temperature = system_monitor_->GetCPUTemperature(temperature_file_name); std::filesystem::remove(temperature_file_name); EXPECT_FLOAT_EQ(temperature, 36.6); } -TEST_F(TestSystemStatusNode, CheckMemoryReadings) +TEST_F(TestSystemMonitorNode, CheckMemoryReadings) { - const auto memory = system_status_->GetMemoryUsage(); + const auto memory = system_monitor_->GetMemoryUsage(); EXPECT_TRUE((memory >= 0.0) && (memory <= 100.0)); } -TEST_F(TestSystemStatusNode, CheckDiskReadings) +TEST_F(TestSystemMonitorNode, CheckDiskReadings) { - const auto disk_usage = system_status_->GetDiskUsage(); + const auto disk_usage = system_monitor_->GetDiskUsage(); EXPECT_TRUE((disk_usage >= 0.0) && (disk_usage <= 100.0)); } -TEST_F(TestSystemStatusNode, CheckSystemStatusToMessage) +TEST_F(TestSystemMonitorNode, CheckSystemStatusToMessage) { - panther_diagnostics::SystemStatusNode::SystemStatus status; + panther_diagnostics::SystemMonitorNode::SystemStatus status; status.core_usages = {50.0, 50.0, 50.0}; status.cpu_mean_usage = 50.0; status.cpu_temperature = 36.6; status.disk_usage = 60.0; status.memory_usage = 30.0; - const auto message = system_status_->SystemStatusToMessage(status); + const auto message = system_monitor_->SystemStatusToMessage(status); EXPECT_EQ(message.cpu_percent, status.core_usages); EXPECT_FLOAT_EQ(message.avg_load_percent, status.cpu_mean_usage); From ffb7ebe94495d8e3b3ec4dcf27a00d62f1531cd5 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Tue, 6 Aug 2024 14:12:24 +0000 Subject: [PATCH 11/67] Update after changes in panther_diagnostics --- ROS_API.md | 2 +- panther_bringup/launch/bringup.launch.py | 6 +++--- panther_manager/README.md | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ROS_API.md b/ROS_API.md index c7c91471e..28b87c473 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -58,7 +58,7 @@ Below is information about the physical robot API. For the simulation, topics an | 🖥️ | `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)* | -| 🤖 | `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_status_node](./panther_diagnostics/)* | +| 🤖 | `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/)* | ### Topics diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index 20fd2664b..d7468ae23 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -64,13 +64,13 @@ def generate_launch_description(): launch_arguments={"namespace": namespace}.items(), ) - system_status_launch = IncludeLaunchDescription( + system_monitor_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [ FindPackageShare("panther_diagnostics"), "launch", - "system_status.launch.py", + "system_monitor.launch.py", ] ), ), @@ -131,7 +131,7 @@ def generate_launch_description(): declare_use_ekf_arg, welcome_msg, controller_launch, - system_status_launch, + system_monitor_launch, delayed_action, ] diff --git a/panther_manager/README.md b/panther_manager/README.md index 0f0742ccc..77b58f34b 100644 --- a/panther_manager/README.md +++ b/panther_manager/README.md @@ -57,7 +57,7 @@ Node responsible for managing safety features, and software shutdown of componen - `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. -- `system_status` [*panther_msgs/SystemStatus*]: State of the system, including Built-in Computer's CPU temperature and load. +- `system_status` [*panther_msgs/SystemStatus*]: Built-in computer system status, includes the most important computation-related parameters. #### Service Clients (for Default Trees) From 2bb319298fb9d705f51365a639987455c01b3edd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Irzyk?= <108666440+pawelirh@users.noreply.github.com> Date: Wed, 7 Aug 2024 09:05:45 +0200 Subject: [PATCH 12/67] Ros2 increase bt service timeout (#382) * Parametrize and increase service timeout in managers * Format panther API drawio file --- .docs/panther_ros2_api.drawio.svg | 2 +- panther_manager/README.md | 4 ++++ .../config/lights_manager_config.yaml | 3 +++ .../config/safety_manager_config.yaml | 3 +++ .../panther_manager/behavior_tree_utils.hpp | 6 ++---- panther_manager/src/lights_manager_node.cpp | 18 ++++++++++++++++- panther_manager/src/safety_manager_node.cpp | 20 +++++++++++++++++-- .../test/test_behavior_tree_utils.cpp | 7 +++++-- 8 files changed, 53 insertions(+), 10 deletions(-) diff --git a/.docs/panther_ros2_api.drawio.svg b/.docs/panther_ros2_api.drawio.svg index 473b1e497..29cefa8b9 100644 --- a/.docs/panther_ros2_api.drawio.svg +++ b/.docs/panther_ros2_api.drawio.svg @@ -1,4 +1,4 @@ -
panther_localization
panther_localization
panther_controller
panther_controller
panther_lights
panther_lights
panther_hardware_interfaces
panther_hardware_interfaces
panther_battery
panther_battery
panther_manager
panther_manager
panther_diagnostics
panther_diagnostics
PantherSystem
PantherSystem
hardware_controller
hardware_controller
GPIOController
GPIOController
MotorsController
MotorsController
CANopenController
CANopenController
RoboteqDriver
RoboteqDriver
CAN
CAN
EStopManager
EStopManager
battery_driver
battery_driver
RPi GPIO
RPi GPIO
ADC
ADC
imu_broadcaster
imu_broadcaster
joint_state_broadcaster
joint_state_broadcaster
drive_controller
drive_controller
controller_manager
controller_manager
PantherImuSensor

PantherImuSensor
IMU
IMU
ekf_filter
ekf_filter
ImuFilter
ImuFilter
Spatial
Spatial
imu/data
imu/data
odometry/wheels
odometry/wheels
odometry/filtered
odometry/filtered
joint_states
joint_states
cmd_vel
cmd_vel
dynamic_joint_states
dynamic_joint_states
 hardware/e_stop
 hardware/e_stop
hardware/motor_controllers_state
hardware/motor_controllers_state
 hardware/io_state
 hardware/io_state
hardware/e_stop_trigger
hardware/e_stop_trigger
hardware/e_stop_reset
hardware/e_stop_reset
hardware/<hw_component>_enable
hardware/<hw_component>_enable
robot_state_publisher
robot_state_publisher
robot_description
robot_description
/tf
/tf
/tf_static
/tf_static
/tf
/tf
battery/battery_status
battery/battery_status
Bumper Lights
Bumper Lights
lights_driver
lights_driver
lights_controller
lights_controller
lights/channel_1_frame
lights/channel_1_frame
lights/channel_2_frame
lights/channel_2_frame
lights/set_animation
lights/set_animation
lights/set_brightness
lights/set_brightness
safety_manager
safety_manager
system_monitor
system_monitor
system_status
system_status
lights_manager
lights_manager
hardware/led_control_enable
hardware/led_control_enable
/tf
/tf
battery/charging_status
battery/charging_status
GPIODriver
GPIODriver
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
Legend:
Legend:
Nodes
Nodes
Plugin
Plugin
Objects
Objects
Topics
Topics
Services
Services
Data Transfer
Data Trans...Text is not SVG - cannot display \ No newline at end of file +
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/panther_manager/README.md b/panther_manager/README.md index 0f0742ccc..80f18340f 100644 --- a/panther_manager/README.md +++ b/panther_manager/README.md @@ -44,6 +44,8 @@ Node responsible for managing Bumper Lights animation scheduling. - `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. - `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. - `ros_plugin_libs` [*list*, default: **Empty list**]: List with names of ROS plugins that are used in a BT project. - `timer_frequency` [*float*, default: **10.0**]: Frequency **[Hz]** at which lights tree will be ticked. @@ -77,6 +79,8 @@ Node responsible for managing safety features, and software shutdown of componen - `driver.temp.window_len` [*int*, default: **6**]: Moving average window length used to smooth out the temperature readings of each driver. - `fan_turn_off_timeout` [*float*, default: **60.0**]: Minimal time in **[s]**, after which the fan may be turned off. - `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. - `ros_plugin_libs` [*list*, default: **Empty list**]: List with names of ROS plugins that are used in a BT project. - `shutdown_hosts_path` [*string*, default: **None**]: Path to a YAML file containing a list of hosts to request shutdown. To correctly format the YAML file, include a **hosts** field consisting of a list with the following fields: - `command` [*string*, default: **sudo shutdown now**]: Command executed on shutdown of given device. diff --git a/panther_manager/config/lights_manager_config.yaml b/panther_manager/config/lights_manager_config.yaml index 03f4ab4b6..cea2a5e4c 100644 --- a/panther_manager/config/lights_manager_config.yaml +++ b/panther_manager/config/lights_manager_config.yaml @@ -12,6 +12,9 @@ low: 30.0 critical: 15.0 charging_anim_step: 0.1 + ros_communication_timeout: + availability: 1.0 + response: 1.0 plugin_libs: - tick_after_timeout_bt_node ros_plugin_libs: diff --git a/panther_manager/config/safety_manager_config.yaml b/panther_manager/config/safety_manager_config.yaml index 3cd648b07..d82d45c7b 100644 --- a/panther_manager/config/safety_manager_config.yaml +++ b/panther_manager/config/safety_manager_config.yaml @@ -16,6 +16,9 @@ window_len: 6 fan_on: 50.0 fan_off: 45.0 + ros_communication_timeout: + availability: 1.0 + response: 1.0 plugin_libs: - tick_after_timeout_bt_node - shutdown_single_host_bt_node diff --git a/panther_manager/include/panther_manager/behavior_tree_utils.hpp b/panther_manager/include/panther_manager/behavior_tree_utils.hpp index 5a57f10a8..360e174e8 100644 --- a/panther_manager/include/panther_manager/behavior_tree_utils.hpp +++ b/panther_manager/include/panther_manager/behavior_tree_utils.hpp @@ -57,18 +57,16 @@ inline void RegisterBehaviorTree( * @param bt_project_path The path to the BehaviorTree project file. * @param plugin_libs A vector containing the names of the nodes that will be registered from * plugins. - * @param node The ROS node used with ROS plugins. + * @param params The ROS-related parameters to register nodes. * @param ros_plugin_libs A vector containing the names of the ROS nodes that will be registered * from plugins. */ inline void RegisterBehaviorTree( BT::BehaviorTreeFactory & factory, const std::string & bt_project_path, - const std::vector plugin_libs, const rclcpp::Node::SharedPtr & node, + const std::vector plugin_libs, const BT::RosNodeParams & params, const std::vector ros_plugin_libs) { for (const auto & plugin : ros_plugin_libs) { - BT::RosNodeParams params; - params.nh = node; RegisterRosNode(factory, BT::SharedLibrary::getOSName(plugin), params); } diff --git a/panther_manager/src/lights_manager_node.cpp b/panther_manager/src/lights_manager_node.cpp index 18df159f0..53ff612bd 100644 --- a/panther_manager/src/lights_manager_node.cpp +++ b/panther_manager/src/lights_manager_node.cpp @@ -23,6 +23,7 @@ #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" @@ -89,6 +90,8 @@ void LightsManagerNode::DeclareParameters() 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); @@ -102,11 +105,24 @@ void LightsManagerNode::DeclareParameters() void LightsManagerNode::RegisterBehaviorTree() { const auto bt_project_path = this->get_parameter("bt_project_path").as_string(); + 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 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(); + + BT::RosNodeParams params; + params.nh = this->shared_from_this(); + params.wait_for_server_timeout = + std::chrono::milliseconds(static_cast(service_availability_timeout * 1000)); + params.server_timeout = + std::chrono::milliseconds(static_cast(service_response_timeout * 1000)); + behavior_tree_utils::RegisterBehaviorTree( - factory_, bt_project_path, plugin_libs, this->shared_from_this(), ros_plugin_libs); + factory_, bt_project_path, plugin_libs, params, ros_plugin_libs); RCLCPP_INFO( this->get_logger(), "BehaviorTree registered from path '%s'", bt_project_path.c_str()); diff --git a/panther_manager/src/safety_manager_node.cpp b/panther_manager/src/safety_manager_node.cpp index 171de7da2..d204690ce 100644 --- a/panther_manager/src/safety_manager_node.cpp +++ b/panther_manager/src/safety_manager_node.cpp @@ -25,6 +25,7 @@ #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" @@ -119,9 +120,11 @@ void SafetyManagerNode::DeclareParameters() 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("shutdown_hosts_path", ""); + 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); @@ -137,11 +140,24 @@ void SafetyManagerNode::DeclareParameters() void SafetyManagerNode::RegisterBehaviorTree() { const auto bt_project_path = this->get_parameter("bt_project_path").as_string(); + 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 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(); + + BT::RosNodeParams params; + params.nh = this->shared_from_this(); + params.wait_for_server_timeout = + std::chrono::milliseconds(static_cast(service_availability_timeout * 1000)); + params.server_timeout = + std::chrono::milliseconds(static_cast(service_response_timeout * 1000)); + behavior_tree_utils::RegisterBehaviorTree( - factory_, bt_project_path, plugin_libs, this->shared_from_this(), ros_plugin_libs); + factory_, bt_project_path, plugin_libs, params, ros_plugin_libs); RCLCPP_INFO( this->get_logger(), "BehaviorTree registered from path '%s'", bt_project_path.c_str()); diff --git a/panther_manager/test/test_behavior_tree_utils.cpp b/panther_manager/test/test_behavior_tree_utils.cpp index 4b24f6034..75b0cb0a4 100644 --- a/panther_manager/test/test_behavior_tree_utils.cpp +++ b/panther_manager/test/test_behavior_tree_utils.cpp @@ -23,6 +23,7 @@ #include "gtest/gtest.h" #include "behaviortree_cpp/tree_node.h" +#include "behaviortree_ros2/ros_node_params.hpp" #include "rclcpp/rclcpp.hpp" #include "panther_manager/behavior_tree_utils.hpp" @@ -100,10 +101,12 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeROS) BT::BehaviorTreeFactory factory; rclcpp::init(0, nullptr); - auto node = std::make_shared("test_node"); + + BT::RosNodeParams params; + params.nh = std::make_shared("test_node"); EXPECT_NO_THROW(panther_manager::behavior_tree_utils::RegisterBehaviorTree( - factory, bt_project_path_, {}, node, + factory, bt_project_path_, {}, params, {"call_trigger_service_bt_node", "call_set_bool_service_bt_node"})); // check if nodes were registered From 5d2bfb196c26d7a6cebf8c8c81f2c291f1ab6013 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Wed, 7 Aug 2024 10:30:59 +0000 Subject: [PATCH 13/67] Rename config and launch file in manager package --- panther_bringup/launch/bringup.launch.py | 2 +- panther_gazebo/launch/simulate_robot.launch.py | 2 +- panther_manager/README.md | 6 +++--- .../{lights_manager_config.yaml => lights_manager.yaml} | 0 .../{safety_manager_config.yaml => safety_manager.yaml} | 0 .../launch/{manager_bt.launch.py => manager.launch.py} | 4 ++-- 6 files changed, 7 insertions(+), 7 deletions(-) rename panther_manager/config/{lights_manager_config.yaml => lights_manager.yaml} (100%) rename panther_manager/config/{safety_manager_config.yaml => safety_manager.yaml} (100%) rename panther_manager/launch/{manager_bt.launch.py => manager.launch.py} (98%) diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index d7468ae23..96fc04453 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -108,7 +108,7 @@ def generate_launch_description(): manager_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_manager"), "launch", "manager_bt.launch.py"] + [FindPackageShare("panther_manager"), "launch", "manager.launch.py"] ) ), condition=UnlessCondition(disable_manager), diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index 512b3a452..55d120be8 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -105,7 +105,7 @@ def generate_launch_description(): manager_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_manager"), "launch", "manager_bt.launch.py"] + [FindPackageShare("panther_manager"), "launch", "manager.launch.py"] ) ), launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), diff --git a/panther_manager/README.md b/panther_manager/README.md index 77b58f34b..9bc8e94bb 100644 --- a/panther_manager/README.md +++ b/panther_manager/README.md @@ -6,7 +6,7 @@ A package containing nodes responsible for high-level control of Husarion Panthe This package contains: -- `manager_bt.launch.py`: Responsible for launching behavior trees responsible for safety and LED animations scheduling. +- `manager.launch.py`: Responsible for launching behavior trees responsible for safety and LED animations scheduling. ## Configuration Files @@ -15,8 +15,8 @@ This package contains: - [`PantherSafetyBT.btproj`](./behavior_trees/PantherSafetyBT.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_config.yaml`](./config/lights_manager_config.yaml): Contains parameters for the `lights_manager` node. -- [`safety_manager_config.yaml`](./config/safety_manager_config.yaml): Contains parameters for the `safety_manager` node. +- [`lights_manager.yaml`](./config/lights_manager.yaml): Contains parameters for the `lights_manager` node. +- [`safety_manager.yaml`](./config/safety_manager.yaml): Contains parameters for the `safety_manager` node. - [`shutdown_hosts.yaml`](./config/shutdown_hosts.yaml): List with all hosts to request shutdown. ## ROS Nodes diff --git a/panther_manager/config/lights_manager_config.yaml b/panther_manager/config/lights_manager.yaml similarity index 100% rename from panther_manager/config/lights_manager_config.yaml rename to panther_manager/config/lights_manager.yaml diff --git a/panther_manager/config/safety_manager_config.yaml b/panther_manager/config/safety_manager.yaml similarity index 100% rename from panther_manager/config/safety_manager_config.yaml rename to panther_manager/config/safety_manager.yaml diff --git a/panther_manager/launch/manager_bt.launch.py b/panther_manager/launch/manager.launch.py similarity index 98% rename from panther_manager/launch/manager_bt.launch.py rename to panther_manager/launch/manager.launch.py index 0eecf65ed..4a91aa7c5 100644 --- a/panther_manager/launch/manager_bt.launch.py +++ b/panther_manager/launch/manager.launch.py @@ -83,7 +83,7 @@ def generate_launch_description(): executable="lights_manager_node", name="lights_manager", parameters=[ - PathJoinSubstitution([panther_manager_dir, "config", "lights_manager_config.yaml"]), + PathJoinSubstitution([panther_manager_dir, "config", "lights_manager.yaml"]), {"bt_project_path": lights_bt_project_path}, ], namespace=namespace, @@ -95,7 +95,7 @@ def generate_launch_description(): executable="safety_manager_node", name="safety_manager", parameters=[ - PathJoinSubstitution([panther_manager_dir, "config", "safety_manager_config.yaml"]), + PathJoinSubstitution([panther_manager_dir, "config", "safety_manager.yaml"]), { "bt_project_path": safety_bt_project_path, "shutdown_hosts_path": shutdown_hosts_config_path, From 04334abcdf90530ce9288f0c845990364ab9cc8c Mon Sep 17 00:00:00 2001 From: pawelirh Date: Wed, 7 Aug 2024 10:32:30 +0000 Subject: [PATCH 14/67] Correct include guards in manager package --- .../include/panther_diagnostics/system_monitor_node.hpp | 6 +++--- .../plugins/action/call_set_bool_service_node.hpp | 6 +++--- .../plugins/action/call_set_led_animation_service_node.hpp | 6 +++--- .../plugins/action/call_trigger_service_node.hpp | 6 +++--- .../plugins/action/shutdown_hosts_from_file_node.hpp | 6 +++--- .../plugins/action/shutdown_single_host_node.hpp | 6 +++--- .../panther_manager/plugins/action/signal_shutdown_node.hpp | 6 +++--- .../plugins/decorator/tick_after_timeout_node.hpp | 6 +++--- .../include/panther_manager/plugins/shutdown_host.hpp | 6 +++--- .../include/panther_manager/plugins/shutdown_hosts_node.hpp | 6 +++--- 10 files changed, 30 insertions(+), 30 deletions(-) diff --git a/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp b/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp index 6292165b1..8728bf738 100644 --- a/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp +++ b/panther_diagnostics/include/panther_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 PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ +#define PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ #include @@ -67,4 +67,4 @@ class SystemMonitorNode : public rclcpp::Node static constexpr char kTemperatureInfoFilename[] = "/sys/class/thermal/thermal_zone0/temp"; }; } // namespace panther_diagnostics -#endif // PANTHER_DIAGNOSTICS__SYSTEM_MONITOR_NODE_HPP_ +#endif // PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp index c5a7e2a00..adafe1d46 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp +++ b/panther_manager/include/panther_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_CALL_SET_BOOL_SERVICE_NODE_HPP_ -#define PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#ifndef PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#define PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_BOOL_SERVICE_NODE_HPP_ #include @@ -45,4 +45,4 @@ class CallSetBoolService : public BT::RosServiceNode } // namespace panther_manager -#endif // PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#endif // PANTHER_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/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp index 234eabc02..38a915a5a 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp +++ b/panther_manager/include/panther_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_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ -#define PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ +#ifndef PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ +#define PANTHER_MANAGER_PLUGINS_ACTION_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ #include @@ -49,4 +49,4 @@ class CallSetLedAnimationService : public BT::RosServiceNode @@ -42,4 +42,4 @@ class CallTriggerService : public BT::RosServiceNode } // namespace panther_manager -#endif // PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#endif // PANTHER_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/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp index 5597d9eb5..949565f61 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp +++ b/panther_manager/include/panther_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_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ -#define PANTHER_MANAGER_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ +#ifndef PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ +#define PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ #include #include @@ -51,4 +51,4 @@ class ShutdownHostsFromFile : public ShutdownHosts } // namespace panther_manager -#endif // PANTHER_MANAGER_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ +#endif // PANTHER_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/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp index 2d2f9ca84..630db6505 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp +++ b/panther_manager/include/panther_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_SHUTDOWN_SINGLE_HOST_NODE_HPP_ -#define PANTHER_MANAGER_SHUTDOWN_SINGLE_HOST_NODE_HPP_ +#ifndef PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_ +#define PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_ #include #include @@ -54,4 +54,4 @@ class ShutdownSingleHost : public ShutdownHosts } // namespace panther_manager -#endif // PANTHER_MANAGER_SHUTDOWN_SINGLE_HOST_NODE_HPP_ +#endif // PANTHER_MANAGER_PLUGINS_ACTION_SHUTDOWN_SINGLE_HOST_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp b/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp index 7d39a5c20..7bc9709db 100644 --- a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp +++ b/panther_manager/include/panther_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_SIGNAL_SHUTDOWN_NODE_HPP_ -#define PANTHER_MANAGER_SIGNAL_SHUTDOWN_NODE_HPP_ +#ifndef PANTHER_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_ +#define PANTHER_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_ #include @@ -44,4 +44,4 @@ class SignalShutdown : public BT::SyncActionNode } // namespace panther_manager -#endif // PANTHER_MANAGER_SIGNAL_SHUTDOWN_NODE_HPP_ +#endif // PANTHER_MANAGER_PLUGINS_ACTION_SIGNAL_SHUTDOWN_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp index 4dd24cd36..20a273e5f 100644 --- a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp +++ b/panther_manager/include/panther_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_TICK_AFTER_TIMEOUT_NODE_HPP_ -#define PANTHER_MANAGER_TICK_AFTER_TIMEOUT_NODE_HPP_ +#ifndef PANTHER_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_ +#define PANTHER_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_ #include #include @@ -42,4 +42,4 @@ class TickAfterTimeout : public BT::DecoratorNode }; } // namespace panther_manager -#endif // PANTHER_MANAGER_TICK_AFTER_TIMEOUT_NODE_HPP_ +#endif // PANTHER_MANAGER_PLUGINS_DECORATOR_TICK_AFTER_TIMEOUT_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp index 74caccfd6..ce29367a1 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp +++ b/panther_manager/include/panther_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_SHUTDOWN_HOST_HPP_ -#define PANTHER_MANAGER_SHUTDOWN_HOST_HPP_ +#ifndef PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_ +#define PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_ #include #include @@ -270,4 +270,4 @@ class ShutdownHost } // namespace panther_manager -#endif // PANTHER_MANAGER_SHUTDOWN_HOST_NODE_HPP_ +#endif // PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOST_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp index 4e3400448..004c94a45 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp +++ b/panther_manager/include/panther_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_SHUTDOWN_HOSTS_NODE_HPP_ -#define PANTHER_MANAGER_SHUTDOWN_HOSTS_NODE_HPP_ +#ifndef PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_ +#define PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_ #include #include @@ -179,4 +179,4 @@ class ShutdownHosts : public BT::StatefulActionNode } // namespace panther_manager -#endif // PANTHER_MANAGER_SHUTDOWN_HOSTS_NODE_HPP_ +#endif // PANTHER_MANAGER_PLUGINS_SHUTDOWN_HOSTS_NODE_HPP_ From a3677ddbbe4641303da013fce50de7f684cf0a05 Mon Sep 17 00:00:00 2001 From: pawelirh Date: Wed, 7 Aug 2024 10:36:40 +0000 Subject: [PATCH 15/67] Restructure files tree in manager tests --- panther_manager/CMakeLists.txt | 71 ++++++++++--------- .../test_call_set_bool_service_node.cpp | 2 +- ...st_call_set_led_animation_service_node.cpp | 2 +- .../test_call_trigger_service_node.cpp | 2 +- .../test_shutdown_hosts_from_file_node.cpp | 2 +- .../test_shutdown_single_host_node.cpp | 2 +- .../test_signal_shutdown_node.cpp | 2 +- .../test_tick_after_timeout_node.cpp | 2 +- .../test/plugins/src/plugin_test_utils.cpp | 69 ------------------ .../test/test_lights_behavior_tree.cpp | 2 +- .../test/test_safety_behavior_tree.cpp | 2 +- .../behavior_tree_test_utils.hpp | 6 +- .../include => utils}/plugin_test_utils.hpp | 48 +++++++++++-- 13 files changed, 91 insertions(+), 121 deletions(-) rename panther_manager/test/plugins/{ => action}/test_call_set_bool_service_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_call_set_led_animation_service_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_call_trigger_service_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_shutdown_hosts_from_file_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_shutdown_single_host_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_signal_shutdown_node.cpp (98%) rename panther_manager/test/plugins/{ => decorator}/test_tick_after_timeout_node.cpp (98%) delete mode 100644 panther_manager/test/plugins/src/plugin_test_utils.cpp rename panther_manager/test/{include => utils}/behavior_tree_test_utils.hpp (89%) rename panther_manager/test/{plugins/include => utils}/plugin_test_utils.hpp (79%) diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 05bc84e6e..b0e68b7d1 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -96,52 +96,51 @@ install(DIRECTORY behavior_trees config launch if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(PkgConfig REQUIRED) + pkg_check_modules(LIBSSH REQUIRED libssh) + find_package(yaml-cpp REQUIRED) - add_library(${PROJECT_NAME}_test_plugin_utils - test/plugins/src/plugin_test_utils.cpp) - target_include_directories( - ${PROJECT_NAME}_test_plugin_utils - PUBLIC $ - $ test/plugins/include) - ament_target_dependencies(${PROJECT_NAME}_test_plugin_utils - ${PACKAGE_DEPENDENCIES}) - target_link_libraries( - ${PROJECT_NAME}_test_plugin_utils - call_set_bool_service_bt_node - call_trigger_service_bt_node - call_set_led_animation_service_bt_node - signal_shutdown_bt_node - shutdown_single_host_bt_node - shutdown_hosts_from_file_bt_node - tick_after_timeout_bt_node) - - ament_add_gtest(${PROJECT_NAME}_test_call_set_bool_service_node - test/plugins/test_call_set_bool_service_node.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_call_set_bool_service_node + test/plugins/action/test_call_set_bool_service_node.cpp + plugins/action/call_set_bool_service_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_call_set_bool_service_node) - ament_add_gtest(${PROJECT_NAME}_test_call_trigger_service_node - test/plugins/test_call_trigger_service_node.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_call_trigger_service_node + test/plugins/action/test_call_trigger_service_node.cpp + plugins/action/call_trigger_service_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_call_trigger_service_node) - ament_add_gtest(${PROJECT_NAME}_test_call_set_led_animation_service_node - test/plugins/test_call_set_led_animation_service_node.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_call_set_led_animation_service_node + test/plugins/action/test_call_set_led_animation_service_node.cpp + plugins/action/call_set_led_animation_service_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_call_set_led_animation_service_node) - ament_add_gtest(${PROJECT_NAME}_test_signal_shutdown_node - test/plugins/test_signal_shutdown_node.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_signal_shutdown_node + test/plugins/action/test_signal_shutdown_node.cpp + plugins/action/signal_shutdown_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_signal_shutdown_node) - ament_add_gtest(${PROJECT_NAME}_test_shutdown_single_host_node - test/plugins/test_shutdown_single_host_node.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_shutdown_single_host_node + test/plugins/action/test_shutdown_single_host_node.cpp + plugins/action/shutdown_single_host_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_single_host_node) - ament_add_gtest(${PROJECT_NAME}_test_shutdown_hosts_from_file_node - test/plugins/test_shutdown_hosts_from_file_node.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_shutdown_hosts_from_file_node + test/plugins/action/test_shutdown_hosts_from_file_node.cpp + plugins/action/shutdown_hosts_from_file_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_hosts_from_file_node) - ament_add_gtest(${PROJECT_NAME}_test_tick_after_timeout_node - test/plugins/test_tick_after_timeout_node.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_tick_after_timeout_node + test/plugins/decorator/test_tick_after_timeout_node.cpp + plugins/decorator/tick_after_timeout_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_tick_after_timeout_node) ament_add_gtest(${PROJECT_NAME}_test_shutdown_host @@ -153,8 +152,14 @@ if(BUILD_TESTING) list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_hosts_node) foreach(bt_node_test ${plugin_tests}) - target_link_libraries(${bt_node_test} ${PROJECT_NAME}_test_plugin_utils) + target_include_directories( + ${bt_node_test} + PUBLIC $ + $ + PRIVATE ${LIBSSH_INCLUDE_DIRS}) + ament_target_dependencies(${bt_node_test} ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${bt_node_test} ${LIBSSH_LIBRARIES} yaml-cpp) endforeach() ament_add_gtest(${PROJECT_NAME}_test_behavior_tree_utils diff --git a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp b/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp similarity index 99% rename from panther_manager/test/plugins/test_call_set_bool_service_node.cpp rename to panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp index 7c5b44b68..c9526698b 100644 --- a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp +++ b/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "panther_manager/plugins/action/call_set_bool_service_node.hpp" -#include "plugin_test_utils.hpp" +#include "utils/plugin_test_utils.hpp" class TestCallSetBoolService : public panther_manager::plugin_test_utils::PluginTestUtils { diff --git a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp b/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp similarity index 99% rename from panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp rename to panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp index 03bf01378..d44ff4ec3 100644 --- a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp +++ b/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "panther_manager/plugins/action/call_set_led_animation_service_node.hpp" -#include "plugin_test_utils.hpp" +#include "utils/plugin_test_utils.hpp" class TestCallSetLedAnimationService : public panther_manager::plugin_test_utils::PluginTestUtils { diff --git a/panther_manager/test/plugins/test_call_trigger_service_node.cpp b/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp similarity index 99% rename from panther_manager/test/plugins/test_call_trigger_service_node.cpp rename to panther_manager/test/plugins/action/test_call_trigger_service_node.cpp index ffc9992d9..e0aa30502 100644 --- a/panther_manager/test/plugins/test_call_trigger_service_node.cpp +++ b/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "panther_manager/plugins/action/call_trigger_service_node.hpp" -#include "plugin_test_utils.hpp" +#include "utils/plugin_test_utils.hpp" class TestCallTriggerService : public panther_manager::plugin_test_utils::PluginTestUtils { diff --git a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp b/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp similarity index 99% rename from panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp rename to panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp index 7e4f600d4..edece3f66 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp +++ b/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp" -#include "plugin_test_utils.hpp" +#include "utils/plugin_test_utils.hpp" typedef panther_manager::plugin_test_utils::PluginTestUtils TestShutdownHostsFromFile; diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp similarity index 99% rename from panther_manager/test/plugins/test_shutdown_single_host_node.cpp rename to panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp index 0ddcc9313..b174b7dc7 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "panther_manager/plugins/action/shutdown_single_host_node.hpp" -#include "plugin_test_utils.hpp" +#include "utils/plugin_test_utils.hpp" typedef panther_manager::plugin_test_utils::PluginTestUtils TestShutdownSingleHost; diff --git a/panther_manager/test/plugins/test_signal_shutdown_node.cpp b/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp similarity index 98% rename from panther_manager/test/plugins/test_signal_shutdown_node.cpp rename to panther_manager/test/plugins/action/test_signal_shutdown_node.cpp index 201d6ca6a..9fd109c27 100644 --- a/panther_manager/test/plugins/test_signal_shutdown_node.cpp +++ b/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "panther_manager/plugins/action/signal_shutdown_node.hpp" -#include "plugin_test_utils.hpp" +#include "utils/plugin_test_utils.hpp" typedef panther_manager::plugin_test_utils::PluginTestUtils TestSignalShutdown; diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp similarity index 98% rename from panther_manager/test/plugins/test_tick_after_timeout_node.cpp rename to panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp index ca1c16717..deba7db91 100644 --- a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "panther_manager/plugins/decorator/tick_after_timeout_node.hpp" -#include "plugin_test_utils.hpp" +#include "utils/plugin_test_utils.hpp" class TestTickAfterTimeout : public panther_manager::plugin_test_utils::PluginTestUtils { diff --git a/panther_manager/test/plugins/src/plugin_test_utils.cpp b/panther_manager/test/plugins/src/plugin_test_utils.cpp deleted file mode 100644 index 7d40fc85a..000000000 --- a/panther_manager/test/plugins/src/plugin_test_utils.cpp +++ /dev/null @@ -1,69 +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 "plugin_test_utils.hpp" - -namespace panther_manager::plugin_test_utils -{ - -PluginTestUtils::PluginTestUtils() -{ - rclcpp::init(0, nullptr); - bt_node_ = std::make_shared("test_panther_manager_node"); -} - -PluginTestUtils::~PluginTestUtils() -{ - bt_node_.reset(); - rclcpp::shutdown(); - if (executor_thread_) { - executor_.reset(); - executor_thread_->join(); - } -} - -std::string PluginTestUtils::BuildBehaviorTree( - const std::string & plugin_name, const std::map & bb_ports) -{ - std::stringstream bt; - - bt << tree_header_ << std::endl; - - bt << "\t\t\t\t<" << plugin_name << " "; - - for (auto const & [key, value] : bb_ports) { - bt << key << "=\"" << value << "\" "; - } - - bt << " />" << std::endl; - - bt << tree_footer_; - - return bt.str(); -} - -void PluginTestUtils::CreateTree( - const std::string & plugin_name, const std::map & bb_ports) -{ - auto xml_text = BuildBehaviorTree(plugin_name, bb_ports); - tree_ = factory_.createTreeFromText(xml_text); -} - -BT::Tree & PluginTestUtils::GetTree() { return tree_; } - -BT::BehaviorTreeFactory & PluginTestUtils::GetFactory() { return factory_; } - -void PluginTestUtils::SpinExecutor() { executor_->spin(); } - -} // namespace panther_manager::plugin_test_utils diff --git a/panther_manager/test/test_lights_behavior_tree.cpp b/panther_manager/test/test_lights_behavior_tree.cpp index 764582858..e96bce136 100644 --- a/panther_manager/test/test_lights_behavior_tree.cpp +++ b/panther_manager/test/test_lights_behavior_tree.cpp @@ -31,8 +31,8 @@ #include "panther_msgs/msg/led_animation.hpp" #include "panther_msgs/srv/set_led_animation.hpp" -#include #include +#include using BoolMsg = std_msgs::msg::Bool; using BatteryStateMsg = sensor_msgs::msg::BatteryState; diff --git a/panther_manager/test/test_safety_behavior_tree.cpp b/panther_manager/test/test_safety_behavior_tree.cpp index d2ff9f8f3..ab7d978a7 100644 --- a/panther_manager/test/test_safety_behavior_tree.cpp +++ b/panther_manager/test/test_safety_behavior_tree.cpp @@ -34,8 +34,8 @@ #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; diff --git a/panther_manager/test/include/behavior_tree_test_utils.hpp b/panther_manager/test/utils/behavior_tree_test_utils.hpp similarity index 89% rename from panther_manager/test/include/behavior_tree_test_utils.hpp rename to panther_manager/test/utils/behavior_tree_test_utils.hpp index 28fc5cece..2a7398113 100644 --- a/panther_manager/test/include/behavior_tree_test_utils.hpp +++ b/panther_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_BEHAVIOR_TREE_TEST_UTILS_ -#define PANTHER_MANAGER_BEHAVIOR_TREE_TEST_UTILS_ +#ifndef PANTHER_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_ +#define PANTHER_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_ #include #include @@ -57,4 +57,4 @@ bool SpinWhileRunning( } // namespace behavior_tree::test_utils -#endif // PANTHER_MANAGER_BEHAVIOR_TREE_TEST_UTILS_ +#endif // PANTHER_MANAGER_TEST_UTILS_BEHAVIOR_TREE_TEST_UTILS_HPP_ diff --git a/panther_manager/test/plugins/include/plugin_test_utils.hpp b/panther_manager/test/utils/plugin_test_utils.hpp similarity index 79% rename from panther_manager/test/plugins/include/plugin_test_utils.hpp rename to panther_manager/test/utils/plugin_test_utils.hpp index ba45989d5..6117a64df 100644 --- a/panther_manager/test/plugins/include/plugin_test_utils.hpp +++ b/panther_manager/test/utils/plugin_test_utils.hpp @@ -52,18 +52,52 @@ struct BehaviorTreePluginDescription class PluginTestUtils : public testing::Test { public: - PluginTestUtils(); - ~PluginTestUtils(); + PluginTestUtils() + { + rclcpp::init(0, nullptr); + bt_node_ = std::make_shared("test_panther_manager_node"); + } + + ~PluginTestUtils() + { + bt_node_.reset(); + rclcpp::shutdown(); + if (executor_thread_) { + executor_.reset(); + executor_thread_->join(); + } + } virtual std::string BuildBehaviorTree( - const std::string & plugin_name, const std::map & bb_ports); + const std::string & plugin_name, const std::map & bb_ports) + { + std::stringstream bt; + + bt << tree_header_ << std::endl; + + bt << "\t\t\t\t<" << plugin_name << " "; + + for (auto const & [key, value] : bb_ports) { + bt << key << "=\"" << value << "\" "; + } + + bt << " />" << std::endl; + + bt << tree_footer_; + + return bt.str(); + } void CreateTree( - const std::string & plugin_name, const std::map & bb_ports); + const std::string & plugin_name, const std::map & bb_ports) + { + auto xml_text = BuildBehaviorTree(plugin_name, bb_ports); + tree_ = factory_.createTreeFromText(xml_text); + } - BT::Tree & GetTree(); + inline BT::Tree & GetTree() { return tree_; } - BT::BehaviorTreeFactory & GetFactory(); + inline BT::BehaviorTreeFactory & GetFactory() { return factory_; } template void CreateService( @@ -105,7 +139,7 @@ class PluginTestUtils : public testing::Test rclcpp::ServiceBase::SharedPtr service; std::unique_ptr executor_thread_; - void SpinExecutor(); + inline void SpinExecutor() { executor_->spin(); } const std::string tree_header_ = R"( From 213b14a69bf6460925f621274a61621ef636f4b5 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Wed, 7 Aug 2024 13:22:14 +0200 Subject: [PATCH 16/67] Ros2 estop sim gui (#384) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * New format of documentation (#369) * Change 3 package for demo * Improve ROS_API * fix links * Update * Update * Table improvements * Format * Save work * Save work * update * fix * fix * fix * fix * fix * Add API warning * Improve links * lights simplify * Create CONFIGURATION.md files * Typos * pre-commit * Apply suggestions from code review Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Save work * Final unification * Delete trash * typos * Update README.md * Update ROS_API.md * Update ROS_API.md * Update README.md Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Change initial warning to beta warning * improve warn rendering * rendering * Update Diagram * Add Dawid suggestions * Dot * Change diagram ext and typos * Do not describe external nodes * Add Dawid suggestons * Add last Dawid suggestions * Format * Pawel suggestions * Diagram improvements * Update * Diagram Visual --------- Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Change scheme theme (#380) * unify CMakeLists.txt files (#381) * First working version * Ros2 increase bt service timeout (#382) * Parametrize and increase service timeout in managers * Format panther API drawio file * Add Estop GUI and docs --------- Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- .docs/panther_ros2_api.drawio.svg | 4 + .pre-commit-config.yaml | 2 +- README.md | 53 + ROS_API.md | 122 ++ panther/panther_simulation.repos | 2 +- panther_battery/CMakeLists.txt | 46 +- panther_battery/README.md | 65 +- panther_bringup/CMakeLists.txt | 2 +- panther_bringup/README.md | 159 +-- panther_bringup/launch/bringup.launch.py | 4 +- panther_controller/CMakeLists.txt | 2 +- panther_controller/CONFIGURATION.md | 9 + panther_controller/README.md | 121 +- .../launch/controller.launch.py | 8 +- panther_description/CMakeLists.txt | 3 +- panther_description/README.md | 11 + .../panther_description.sh.in | 0 .../launch/load_urdf.launch.py | 8 +- .../urdf/panther_macro.urdf.xacro | 2 +- panther_diagnostics/CMakeLists.txt | 36 +- panther_diagnostics/README.md | 39 +- panther_diagnostics/cmake/SuperBuild.cmake | 2 +- panther_gazebo/CMakeLists.txt | 99 +- panther_gazebo/CONFIGURATION.md | 61 + panther_gazebo/README.md | 104 +- panther_gazebo/config/gz_bridge.yaml | 6 +- .../config/teleop_with_estop.config | 1266 +++++++++++++++++ panther_gazebo/hooks/panther_gazebo.sh.in | 1 + .../include/panther_gazebo/gui/Estop.hpp | 63 + .../launch/simulate_multiple_robots.launch.py | 1 + .../launch/simulate_robot.launch.py | 2 +- panther_gazebo/launch/simulation.launch.py | 33 +- panther_gazebo/src/gui/Estop.config | 2 + panther_gazebo/src/gui/Estop.cpp | 93 ++ panther_gazebo/src/gui/Estop.qml | 84 ++ panther_gazebo/src/gui/Estop.qrc | 5 + panther_hardware_interfaces/CMakeLists.txt | 58 +- panther_hardware_interfaces/README.md | 92 +- panther_lights/CMakeLists.txt | 36 +- panther_lights/CONFIGURATION.md | 167 +++ panther_lights/README.md | 277 +--- panther_localization/CMakeLists.txt | 2 +- panther_localization/README.md | 50 +- .../launch/localization.launch.py | 4 +- panther_manager/CMakeLists.txt | 22 +- panther_manager/CONFIGURATION.md | 211 +++ panther_manager/README.md | 365 +---- .../config/lights_manager_config.yaml | 3 + .../config/safety_manager_config.yaml | 3 + .../config/shutdown_hosts.yaml | 0 .../panther_manager/behavior_tree_utils.hpp | 6 +- panther_manager/launch/manager_bt.launch.py | 2 +- panther_manager/src/lights_manager_node.cpp | 18 +- panther_manager/src/safety_manager_node.cpp | 20 +- .../test/test_behavior_tree_utils.cpp | 7 +- panther_utils/CMakeLists.txt | 28 +- 56 files changed, 2604 insertions(+), 1287 deletions(-) create mode 100644 .docs/panther_ros2_api.drawio.svg create mode 100644 ROS_API.md create mode 100644 panther_controller/CONFIGURATION.md rename panther_description/{env-hooks => hooks}/panther_description.sh.in (100%) create mode 100644 panther_gazebo/CONFIGURATION.md create mode 100644 panther_gazebo/config/teleop_with_estop.config create mode 100644 panther_gazebo/hooks/panther_gazebo.sh.in create mode 100644 panther_gazebo/include/panther_gazebo/gui/Estop.hpp create mode 100644 panther_gazebo/src/gui/Estop.config create mode 100644 panther_gazebo/src/gui/Estop.cpp create mode 100644 panther_gazebo/src/gui/Estop.qml create mode 100644 panther_gazebo/src/gui/Estop.qrc create mode 100644 panther_lights/CONFIGURATION.md create mode 100644 panther_manager/CONFIGURATION.md rename {panther_bringup => panther_manager}/config/shutdown_hosts.yaml (100%) diff --git a/.docs/panther_ros2_api.drawio.svg b/.docs/panther_ros2_api.drawio.svg new file mode 100644 index 000000000..29cefa8b9 --- /dev/null +++ b/.docs/panther_ros2_api.drawio.svg @@ -0,0 +1,4 @@ + + + +
panther_localization
panther_localization
panther_controller
panther_controller
panther_lights
panther_lights
panther_hardware_interfaces
panther_hardware_interfaces
panther_battery
panther_battery
panther_manager
panther_manager
panther_diagnostics
panther_diagnostics
PantherSystem
PantherSystem
hardware_controller
hardware_controller
GPIOController
GPIOController
MotorsController
MotorsController
CANopenController
CANopenController
RoboteqDriver
RoboteqDriver
CAN
CAN
EStopManager
EStopManager
battery_driver
battery_driver
RPi GPIO
RPi GPIO
ADC
ADC
imu_broadcaster
imu_broadcaster
joint_state_broadcaster
joint_state_broadcaster
drive_controller
drive_controller
controller_manager
controller_manager
PantherImuSensor

PantherImuSensor
IMU
IMU
ekf_filter
ekf_filter
ImuFilter
ImuFilter
Spatial
Spatial
imu/data
imu/data
odometry/wheels
odometry/wheels
odometry/filtered
odometry/filtered
joint_states
joint_states
cmd_vel
cmd_vel
dynamic_joint_states
dynamic_joint_states
 hardware/e_stop
 hardware/e_stop
hardware/motor_controllers_state
hardware/motor_controllers_state
 hardware/io_state
 hardware/io_state
hardware/e_stop_trigger
hardware/e_stop_trigger
hardware/e_stop_reset
hardware/e_stop_reset
hardware/<hw_component>_enable
hardware/<hw_component>_enable
robot_state_publisher
robot_state_publisher
robot_description
robot_description
/tf
/tf
/tf_static
/tf_static
/tf
/tf
battery/battery_status
battery/battery_status
Bumper Lights
Bumper Lights
lights_driver
lights_driver
lights_controller
lights_controller
lights/channel_1_frame
lights/channel_1_frame
lights/channel_2_frame
lights/channel_2_frame
lights/set_animation
lights/set_animation
lights/set_brightness
lights/set_brightness
safety_manager
safety_manager
system_monitor
system_monitor
system_status
system_status
lights_manager
lights_manager
hardware/led_control_enable
hardware/led_control_enable
/tf
/tf
battery/charging_status
battery/charging_status
GPIODriver
GPIODriver
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
diagnostics
Legend:
Legend:
Nodes
Nodes
Plugin
Plugin
Objects
Objects
Topics
Topics
Services
Services
Data Transfer
Data Trans...Text is not SVG - cannot display diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 66c65de5b..421c74955 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -51,7 +51,7 @@ repos: "--ignore-words-list", "ned" # north, east, down (NED) ] - exclude_types: [rst] + exclude_types: [rst, svg] language: python types: [text] diff --git a/README.md b/README.md index f738d8fc4..1e2ce7440 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,11 @@ ROS 2 packages for Panther autonomous mobile robot [![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit)](https://github.com/pre-commit/pre-commit) + + + Panther preview + + ## Quick start ### Create workspace @@ -66,6 +71,54 @@ Simulation: ros2 launch panther_gazebo simulation.launch.py ``` +### Launch Arguments + +Launch arguments are largely common to both simulation and physical robot. However, there is a group of arguments that apply only to hardware or only to the simulator. Below is a legend to the tables with all launch arguments. + +| Symbol | Meaning | +| ------ | ---------------------------- | +| 🤖 | Available for physical robot | +| 🖥️ | Available in simulation | + +| | Argument | Description
***Type:*** `Default` | +| --- | ---------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🖥️ | `add_wheel_joints` | Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information.
***bool:*** `False` | +| 🖥️ | `add_world_transform` | Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots).
***bool:*** `False` | +| 🖥️ | `battery_config_path` | Path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only.
***string:*** `None` | +| 🤖🖥️ | `components_config_path` | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options).
***string:*** [`components.yaml`](./panther_description/config/components.yaml) | +| 🤖🖥️ | `controller_config_path` | Path to controller configuration file. A path to custom configuration can be specified here.
***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/) | +| 🤖 | `disable_manager` | Enable or disable manager_bt_node.
***bool:*** `False` | +| 🤖🖥️ | `fuse_gps` | Include GPS for data fusion.
***bool:*** `False` | +| 🖥️ | `gz_bridge_config_path` | Path to the parameter_bridge configuration file.
***string:*** [`gz_bridge.yaml`](./panther_gazebo/config/gz_bridge.yaml) | +| 🖥️ | `gz_gui` | Run simulation with specific GUI layout.
***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config) | +| 🖥️ | `gz_headless_mode` | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations.
***bool:*** `False` | +| 🖥️ | `gz_log_level` | Adjust the level of console output.
***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`) | +| 🖥️ | `gz_world` | Absolute path to SDF world file.
***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf) | +| 🤖🖥️ | `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.yaml`](./panther_manager/config/shutdown_hosts.yaml) | +| 🤖🖥️ | `use_ekf` | Enable or disable EKF.
***bool:*** `True` | +| 🤖🖥️ | `use_sim` | Whether simulation is used.
***bool:*** `False` | +| 🤖🖥️ | `user_led_animations_file` | Path to a YAML file with a description of the user-defined animations.
***string:*** `''` | +| 🤖🖥️ | `wheel_config_path` | Path to wheel configuration file.
***string:*** [`{wheel_type}.yaml`](./panther_description/config) | +| 🤖🖥️ | `wheel_type` | Type of wheel. If you choose a value from the preset options ('WH01', 'WH02', 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path' parameters. For custom wheels, please define these parameters to point to files that accurately describe the custom wheels.
***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) | +| 🖥️ | `x` | Initial robot position in the global 'x' axis.
***float:*** `0.0` | +| 🖥️ | `y` | Initial robot position in the global 'y' axis.
***float:***` -2.0` | +| 🖥️ | `z` | Initial robot position in the global 'z' axis.
***float:*** `0.2` | +| 🖥️ | `roll` | Initial robot 'roll' orientation.
***float:*** `0.0` | +| 🖥️ | `pitch` | Initial robot 'pitch' orientation.
***float:*** `0.0` | +| 🖥️ | `yaw` | Initial robot 'yaw' orientation.
***float:*** `0.0` | + +> [!TIP] +> +> To read the arguments for individual packages, add the `-s` flag to the `ros2 launch` command (e.g. `ros2 launch panther_bringup bringup.launch.py ​​-s`) + ## Developer Info ### Setup pre-commit diff --git a/ROS_API.md b/ROS_API.md new file mode 100644 index 000000000..c7c91471e --- /dev/null +++ b/ROS_API.md @@ -0,0 +1,122 @@ +# ROS API + +> [!WARNING] +> **Beta Release** +> +> Please be advised that the software you are about to use is the Beta version of the ROS 2 Driver for Panther. It is functional and the architecture will not change significantly. It was tested by the Husarion team, however, some stability issues and bugs might still occur. Additionally, the ROS 2 API may face some minor changes in the following releases. +> +> We would be grateful for your feedback related to Panther ROS 2 driver. You can reach us the following ways: +> +> - by email at [support@husarion.com](mailto:support@husarion.com) +> - via our community forum: [Husarion Community](https://community.husarion.com) +> - using issue request on [GitHub](https://github.com/husarion/panther_ros/issues) + +## ROS 2 System Design + +This section describes the ROS packages in the Panther ROS system. These packages are located in the [panther_ros](https://github.com/husarion/panther_ros) GitHub repository. + +> [!NOTE] +> **Differences in ROS System** +> +> ROS 2 nodes differs slightly between **Panther v1.06** and **Panther v1.2+**. This is caused by internal hardware differences. Despite that, the ROS API was kept as closely matched between those revisions as possible and should be transparent in most of the use cases. + + + +The default way to communicate with Panther's hardware is via the Robot Operating System (ROS). All the drivers were written in ROS 2 framework. The ROS API is provided by ROS packages found in the GitHub repository [husarion/panther_ros](https://github.com/husarion/panther_ros). These packages are responsible for accessing the hardware components of the robot. + +The graph below represents Panther's ROS system. Some topics and services have been excluded from the graph for the sake of clarity. + +![Panther ROS 2 API Diagram](.docs/panther_ros2_api.drawio.svg) + +## ROS Interfaces + +Below is information about the physical robot API. For the simulation, topics and services are identical to the physical robot, but due to certain limitations, not all interfaces are present in the simulation. + +| Symbol | Meaning | +| ------ | ------------------------------- | +| 🤖 | Available for physical robot | +| 🖥️ | Available in simulation | +| ⚙️ | Requires specific configuration | + +### Nodes + +| | Node name | Description | +| --- | ------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🤖 | `battery_driver` | Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot.
[*panther_batter/battery_node*](./panther_batter) | +| 🤖🖥️ | `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)* | +| 🖥️ | `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) | +| 🤖🖥️ | `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/ControllerNode*](./panther_lights) | +| 🤖 | `lights_driver` | This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights.
[*panther_lights/DriverNode*](./panther_lights) | +| 🤖🖥️ | `lights_manager` | Node responsible for managing Bumper Lights animation scheduling.
[panther_manager/lights_manager](./panther_lights/) | +| 🤖🖥️⚙️ | `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)* | +| 🤖 | `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_status_node](./panther_diagnostics/)* | + +### 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/fix` | Raw GPS data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | +| 🤖🖥️⚙️ | `gps/filtered` | Filtered GPS position after fusing odometry data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | +| 🤖 | `hardware/e_stop` | Current E-stop state.
[*std_msgs/Bool*](https://docs.ros.org/en/latest/api/std_msgs/html/msg/Bool.html) | +| 🤖 | `hardware/io_state` | Current IO state.
[*panther_msgs/IOState*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `hardware/motor_controllers_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
[*panther_msgs/DriverState*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `imu/data` | Filtered IMU data.
[*sensor_msgs/Imu*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html) | +| 🤖🖥️ | `joint_states` | Provides information about the state of various joints in a robotic system.
[*sensor_msgs/JointState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html) | +| 🤖🖥️ | `lights/channel_1_frame` | Frame to be displayed on robot Front Bumper Lights.
[*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) | +| 🤖🖥️ | `lights/channel_2_frame` | Frame to be displayed on robot Rear Bumper Lights.
[*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) | +| 🤖🖥️ | `localization/set_pose` | Sets the pose of the EKF node.
[*geometry_msgs/PoseWithCovarianceStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseWithCovarianceStamped.html) | +| 🤖🖥️ | `odometry/filtered` | Contains information about the filtered position and orientation. When `localization_mode` is `relative`, the position and orientation are relative to the starting point. When `localization_mode` is `enu`, the orientation is relative to the east-north-up (ENU) coordinates.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | +| 🤖🖥️ | `odometry/wheels` | Robot odometry calculated from wheels.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | +| 🤖🖥️ | `robot_description` | Contains information about robot description from URDF file.
[*std_msgs/String*](https://docs.ros2.org/latest/api/std_msgs/msg/String.html) | +| 🤖 | `system_status` | State of the system, including Built-in Computer's CPU temperature and load.
[*panther_msgs/SystemStatus*](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `tf` | Transforms of robot system.
[*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html) | +| 🤖🖥️ | `tf_static` | Static transforms of robot system.
[*tf2_msgs/TFMessage*](https://docs.ros2.org/latest/api/tf2_msgs/msg/TFMessage.html) | + +#### Hidden topics + +| | Topic | Description | +| --- | ------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 🤖 | `_battery/battery_1_status_raw` | First battery raw state.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | +| 🤖 | `_battery/battery_2_status_raw` | Second battery raw state. Published if second battery detected.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | +| 🤖🖥️⚙️ | `_odometry/gps` | Transformed raw GPS data to odometry format.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | + +### Services + +| | Service | Description | +| --- | ------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| 🤖🖥️ | `controller_manager/configure_controller` | Manage lifecycle transition.
[controller_manager_msgs/srv/ConfigureController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_controller_types` | Output the available controller types and their base classes.
[controller_manager_msgs/srv/ListControllerTypes](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_controllers` | Output the list of loaded controllers, their type and status.
[controller_manager_msgs/srv/ListControllers](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_hardware_components` | Output the list of available hardware components.
[controller_manager_msgs/srv/ListHardwareComponents](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/list_hardware_interfaces` | Output the list of available command and state interfaces.
[controller_manager_msgs/srv/ListHardwareInterfaces](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/load_controller` | Load a controller in a controller manager.
[controller_manager_msgs/srv/LoadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/reload_controller_libraries` | Reload controller libraries.
[controller_manager_msgs/srv/ReloadControllerLibraries](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/set_hardware_component_state` | Adjust the state of the hardware component.
[controller_manager_msgs/srv/SetHardwareComponentState](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/switch_controller` | Switch controllers in a controller manager.
[controller_manager_msgs/srv/SwitchController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖🖥️ | `controller_manager/unload_controller` | Unload a controller in a controller manager.
[controller_manager_msgs/srv/UnloadController](https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs) | +| 🤖 | `hardware/aux_power_enable` | Enables or disables AUX power.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/charger_enable` | Enables or disables external charger.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/digital_power_enable` | Enables or disables digital power.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/e_stop_reset` | Resets E-stop.
[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) | +| 🤖 | `hardware/e_stop_trigger` | Triggers E-stop.
[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) | +| 🤖 | `hardware/fan_enable` | Enables or disables fan.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖 | `hardware/motor_power_enable` | Enables or disables motor power.
[std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html) | +| 🤖🖥️ | `lights/set_animation` | Sets LED animation.
[panther_msgs/srv/SetLEDAnimation](https://github.com/husarion/panther_msgs) | +| 🤖 | `lights/set_brightness` | Sets global LED brightness, value ranges from **0.0** to **1.0**.
[panther_msgs/SetLEDBrightness](https://github.com/husarion/panther_msgs) | +| 🤖🖥️ | `localization/enable` | Enable EKF node.
[std_srvs/srv/Empty](https://docs.ros2.org/latest/api/std_srvs/srv/Empty.html) | +| 🤖🖥️ | `localization/set_pose` | Set pose of EKF node.
[robot_localization/srv/SetPose](https://github.com/cra-ros-pkg/robot_localization/tree/ros2) | +| 🤖🖥️ | `localization/toggle` | Toggle filter processing in the EKF node.
[robot_localization/srv/ToggleFilterProcessing](https://github.com/cra-ros-pkg/robot_localization/tree/ros2) | diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index b3e2233f7..83df44fa3 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -22,4 +22,4 @@ repositories: husarion_gz_worlds: type: git url: https://github.com/husarion/husarion_gz_worlds.git - version: bba5074af5eabf404850245a70a1d60192912a3b + version: 9d514a09c74bca2afbfba76cf2c87134918bbf97 diff --git a/panther_battery/CMakeLists.txt b/panther_battery/CMakeLists.txt index 9bc58a551..37fd6c814 100644 --- a/panther_battery/CMakeLists.txt +++ b/panther_battery/CMakeLists.txt @@ -5,12 +5,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(panther_msgs REQUIRED) -find_package(panther_utils REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) +set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater panther_msgs + panther_utils rclcpp sensor_msgs) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() include_directories(include ${panther_utils_INCLUDE_DIRS}) @@ -23,9 +23,7 @@ add_executable( src/battery_publisher.cpp src/dual_battery_publisher.cpp src/single_battery_publisher.cpp) - -ament_target_dependencies(battery_node diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) +ament_target_dependencies(battery_node ${PACKAGE_DEPENDENCIES}) install(TARGETS battery_node DESTINATION lib/${PROJECT_NAME}) @@ -73,9 +71,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_publisher diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_single_battery_publisher @@ -85,9 +82,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_single_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_single_battery_publisher diagnostic_updater - panther_msgs panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_single_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_dual_battery_publisher @@ -97,9 +93,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_dual_battery_publisher PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_dual_battery_publisher diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_dual_battery_publisher + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_battery_node @@ -114,9 +109,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_node PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_node + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_battery_node_roboteq @@ -131,9 +125,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_node_roboteq PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node_roboteq diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_node_roboteq + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_battery_node_dual_bat @@ -148,9 +141,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_battery_node_dual_bat PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_battery_node_dual_bat diagnostic_updater panther_msgs - panther_utils rclcpp sensor_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_battery_node_dual_bat + ${PACKAGE_DEPENDENCIES}) endif() diff --git a/panther_battery/README.md b/panther_battery/README.md index ed6bab6d0..2523e3b99 100644 --- a/panther_battery/README.md +++ b/panther_battery/README.md @@ -1,64 +1,39 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_battery -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - -Package containing nodes monitoring and publishing the internal battery state of the Husarion Panther robot. - -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) +The package containing nodes monitoring and publishing the internal battery state of the Husarion Panther robot. -## ROS Nodes +## Launch Files -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +This package contains: -### battery_driver +- `battery.launch.py`: Responsible for activating battery node, which dealing with reading and publishing battery data. -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) +## ROS Nodes -Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot. +### battery_node -[//]: # (ROS_API_NODE_DESCRIPTION_END) +Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier.versions of the robot. #### Publishes -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `_battery/battery_1_status_raw` [*sensor_msgs/BatteryState*]: first battery raw state. -- `_battery/battery_2_status_raw` [*sensor_msgs/BatteryState*]: second battery raw state. Published if second battery detected. -- `battery/battery_status` [*sensor_msgs/BatteryState*]: mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published. -- `battery/charging_status` [*panther_msgs/ChargingStatus*]: battery charging status. -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: battery diagnostic messages. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) +- `_battery/battery_1_status_raw` [*sensor_msgs/BatteryState*]: First battery raw state. +- `_battery/battery_2_status_raw` [*sensor_msgs/BatteryState*]: Second battery raw state. Published if second battery detected. +- `battery/battery_status` [*sensor_msgs/BatteryState*]: Mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published. +- `battery/charging_status` [*panther_msgs/ChargingStatus*]: Battery charging status. +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Battery diagnostic messages. #### Subscribes -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `hardware/io_state` [*panther_msgs/IOState*]: current state of IO. -- `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) +- `hardware/io_state` [*panther_msgs/IOState*]: Current state of IO. +- `hardware/motor_controllers_state` [*panther_msgs/DriverState*]: Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - - `~/adc/device0` [*string*, default: **/sys/bus/iio/devices/iio:device0**]: ADC nr 0 IIO device. Used with Panther version 1.2 and above. - `~/adc/device1` [*string*, default: **/sys/bus/iio/devices/iio:device1**]: ADC nr 1 IIO device. Used with Panther version 1.2 and above. -- `~/adc/ma_window_len/charge` [*int*, default: **10**]: window length of a moving average, used to smooth out battery charge readings. Used with Panther version 1.2 and above. -- `~/adc/ma_window_len/temp` [*int*, default: **10**]: window length of a moving average, used to smooth out battery temperature readings. Used with Panther version 1.2 and above. -- `~/battery_timeout` [*float*, default: **1.0**]: specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state. -- `~/ma_window_len/voltage` [*int*, default: **10**]: window length of a moving average, used to smooth out battery voltage readings. -- `~/ma_window_len/current` [*int*, default: **10**]: window length of a moving average, used to smooth out battery current readings. -- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: specifies timeout in seconds after which driver state messages will be considered old. Used with Panther version 1.06 and earlier. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) +- `~/adc/ma_window_len/charge` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery charge readings. Used with Panther version 1.2 and above. +- `~/adc/ma_window_len/temp` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery temperature readings. Used with Panther version 1.2 and above. +- `~/battery_timeout` [*float*, default: **1.0**]: Specifies the timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state. +- `~/ma_window_len/voltage` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery voltage readings. +- `~/ma_window_len/current` [*int*, default: **10**]: Window length of a moving average, used to smooth out battery current readings. +- `~/roboteq/driver_state_timeout` [*float*, default: **0.2**]: Specifies timeout in seconds after which driver state messages will be considered old. Used with Panther version 1.06 and earlier. diff --git a/panther_bringup/CMakeLists.txt b/panther_bringup/CMakeLists.txt index bd2e92f43..793057508 100644 --- a/panther_bringup/CMakeLists.txt +++ b/panther_bringup/CMakeLists.txt @@ -3,6 +3,6 @@ project(panther_bringup) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_bringup/README.md b/panther_bringup/README.md index dcc85e4ef..b12b4218d 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -1,162 +1,9 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_bringup -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) - -## Default Nodes Launched - -- `battery_driver` [*[panther_battery/battery_node](https://github.com/husarion/panther_ros/panther_battery/src/main.cpp)*]: node responsible for monitoring and publishing the internal Battery state of the Husarion Panther robot. For more information, refer to [panther_battery](https://github.com/husarion/panther_ros/panther_battery/README.md). -- `ekf_filter` [*[robot_localization/ekf_node](https://github.com/cra-ros-pkg/robot_localization/blob/humble-devel/src/ekf_node.cpp)*]: Extended Kalman Filter node for more accurate odometry. For more information, refer to [robot_localization](https://github.com/cra-ros-pkg/robot_localization/tree/noetic-devel). The default configuration is stored in [ekf_config.yaml](https://github.com/husarion/panther_ros/panther_gazebo/config/ekf_config.yaml). -- `imu_container` [*[phidgets_spatial/phidgets::SpatialRosI](https://github.com/ros-drivers/phidgets_drivers/blob/humble/phidgets_spatial/src/spatial_ros_i.cpp)*, *[imu_filter_madgwick/ImuFilterMadgwickRos](https://github.com/CCNYRoboticsLab/imu_tools/blob/humble/imu_filter_madgwick/src/imu_filter_node.cpp)*]: container responsible for running Phidget Spatial IMU ROS driver, filtering and fusing the IMU data. It composes the `phidgets_spatial_node` and `imu_filter_node`. - -## Bringup Launch Arguments - -- `battery_config_path` [*string*, default: **None**]: path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. -- `controller_config_path` [*string*, default: **panther_controller/config/_controller.yaml**]: path to controller configuration file. A path to custom configuration can be specified here. -- `disable_manager` [*bool*, default: **false**]: Enable or disable manager_bt_node. -- `ekf_config_path` [*string*, default: **panther_bringup/config/ekf.yaml**]: path to the EKF configuration file. -- `led_config_file` [*string*, default: **panther_lights/config/led_config.yaml**]: path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. -- `namespace` [*string*, default: **None**] Add namespace to all launched nodes. -- `publish_robot_state` [*bool*, default: **true**]: whether to publish the default Panther robot description. -- `shutdown_hosts_config_path` = [*string*, default: **panther_bringup/config/shutdown_hosts.yaml**]: Path to file with list of hosts to request shutdown. -- `use_ekf` [*bool*, default: **true**]: enable or disable Extended Kalman Filter. -- `use_sim` [*bool*, default: **false**]: whether simulation is used. -- `user_led_animations_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. -- `wheel_config_path` [*string*, default: **$(find panther_description)/config/.yaml**]: path to YAML file with wheel specification. Arguments become required if `wheel_type` is set to **custom**. -- `wheel_type` [*string*, default: **WH01**]: type of wheel, possible are: **WH01** - off-road, **WH02** - mecanum, **WH04** - small pneumatic, and **custom** - custom wheel types (requires setting `wheel_config_path` argument accordingly). -- `components_config_path` [*string*, default: **None**]: Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). Example of configuration you can find [config/components.yaml](config/components.yaml) - -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - -## External ROS Nodes - -[//]: # (ROS_API_PACKAGE_NAME_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### ekf_filter - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type: *[robot_localization/ekf_node](https://github.com/cra-ros-pkg/robot_localization/blob/humble-devel/src/ekf_node.cpp)*. - -Extended Kalman Filter node for more accurate odometry. For more information, refer to [robot_localization](https://github.com/cra-ros-pkg/robot_localization/tree/humble-devel). The default configuration is stored in `panther_bringup/config/ekf.yaml`. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Subscribers - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `~/odometry/wheels` [*nav_msgs/Odometry*]: robot odometry calculated from wheels. -- `~/imu/data` [*sensor_msgs/Imu*]: filtered IMU data. -- `/tf` [*tf2_msgs/TFMessage*]: transforms of robot system. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `~/odometry/filtered` [*nav_msgs/Odometry*]: provides filtered odometry information. This topic contains a fused and enhanced estimate of the robot's pose and velocity, incorporating data from various sensors and correcting for any errors in the estimated state. -- `/tf` [*tf2_msgs/TFMessage*]: publishes `odom` to `base_link` transform. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) - -#### Service Servers - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) - -- `localization/set_pose` [*robot_localization/SetPose*]: by issuing a *geometry_msgs/PoseWithCovarianceStamped* message to the set_pose topic, users can manually set the state of the filter. This is useful for resetting the filter during testing and allows for interaction with RViz. Alternatively, the state estimation nodes advertise a SetPose service, whose type is *robot_localization/SetPose*. - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### imu_filter_node - -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type: *[imu_filter_madgwick/imu_filter_node](https://github.com/CCNYRoboticsLab/imu_tools/blob/humble/imu_filter_madgwick/src/imu_filter_node.cpp)*. - -Node responsible for filtering and fusing raw data from the IMU. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Subscribers - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `~/imu/data_raw` [*sensor_msgs/Imu*]: the raw accelerometer and gyroscope data. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `~/imu/data` [*sensor_msgs/Imu*]: the fused IMU messages, containing the orientation. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### phidgets_spatial_node - -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type: *[phidgets_spatial/spatial_ros_i.cpp](https://github.com/ros-drivers/phidgets_drivers/blob/humble/phidgets_spatial/src/spatial_ros_i.cpp)*. - -The ROS driver for Phidgets Spatial. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `~/imu/data_raw` [*sensor_msgs/Imu*]: the raw accelerometer and gyroscope data. -- `~/imu/is_calibrated` [*std_msgs/Bool*]: whether the gyroscope has been calibrated. This will be done automatically at startup time but can also be re-done at any time by calling the `imu/calibrate` service. -- `~/imu/mag` [*sensor_msgs/MagneticField*]: the raw magnetometer data. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) - -#### Service Servers - -[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) - -- `~/imu/calibrate` [*std_srvs/Empty*]: run calibration on the gyroscope. +## Launch Files -[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) -[//]: # (ROS_API_NODE_END) +This package contains: -[//]: # (ROS_API_PACKAGE_END) +- `bringup.launch.py`: Responsible for activating whole robot system. diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index 8e1b3c12b..20fd2664b 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): "disable_manager", default_value="False", description="Enable or disable manager_bt_node.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) namespace = LaunchConfiguration("namespace") @@ -48,7 +48,7 @@ def generate_launch_description(): "use_ekf", default_value="True", description="Enable or disable EKF.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) serial_no = EnvironmentVariable(name="PANTHER_SERIAL_NO", default_value="----") diff --git a/panther_controller/CMakeLists.txt b/panther_controller/CMakeLists.txt index b71172368..c6489681f 100644 --- a/panther_controller/CMakeLists.txt +++ b/panther_controller/CMakeLists.txt @@ -3,6 +3,6 @@ project(panther_controller) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/panther_controller/CONFIGURATION.md b/panther_controller/CONFIGURATION.md new file mode 100644 index 000000000..f541b8727 --- /dev/null +++ b/panther_controller/CONFIGURATION.md @@ -0,0 +1,9 @@ +# panther_controller + +## Changing Velocity Smoothing Parameters + +The Panther by default uses [diff_drive_controller](https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html) from [ros2 control](https://control.ros.org/master/index.html) or [mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller). This controller can be customized, among others: by modifying the robot's dynamic parameters (e.g. smooth the speed or limit the robot's speed and acceleration). Its parameters can be found in the [panther_controller](https://github.com/husarion/panther_ros/tree/ros2-devel/panther_controller/config). By default, these values correspond to the upper limits of the robot's velocities and accelerations. + +## Changing Wheel Type + +Changing wheel types is possible and can be done for both the real robot and the simulation. By default, three types of wheels are supported using the launch argument `wheel_type`. If you want to use custom wheels, all you need to do is point to the new wheel and controller configuration files using the `wheel_config_path` and `controller_config_path` parameters. These files should be based on the default files, i.e. [WH01_controller.yaml](./config/WH01_controller.yaml) and [WH01.yaml](../panther_description/config/WH01.yaml). diff --git a/panther_controller/README.md b/panther_controller/README.md index dc373312e..ac600dc72 100644 --- a/panther_controller/README.md +++ b/panther_controller/README.md @@ -1,122 +1,13 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_controller -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - The package contains the default configuration and launch files necessary to start all the basic functionalities of the Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) - -## Default Nodes Launched - -- `ros2_control_node` [*[controller_manager/controller_manager](https://github.com/ros-controls/ros2_control/blob/master/controller_manager)*]: Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. For more information, refer to [controller_manager](https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html). This node manages the following controllers: - - `imu_broadcaster` [*[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)*]: The broadcaster to publish readings of IMU sensors. - - `joint_state_broadcaster` [*[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)*]: The broadcaster reads all state interfaces and reports them on specific topics. - - `drive_controller` [*[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller)* ]: Controller which manages mobile robots with a differential drive. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. -- `robot_state_publisher` [*[robot_state_publisher/robot_state_publisher](https://github.com/ros/robot_state_publisher)*]: The Robot State Publisher broadcasts a robot's state to tf2 using a provided URDF model and joint states. It updates the model and broadcasts poses for fixed and movable joints to tf2 topics. - -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - -## External ROS Nodes - -[//]: # (ROS_API_PACKAGE_NAME_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### imu_broadcaster - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type:*[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)*. - -The broadcaster to publish readings of IMU sensors. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `imu/data` [*sensor_msgs/msg/Imu*]: data from IMU sensor. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### joint_state_broadcaster - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type:*[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)*. - -The broadcaster reads all state interfaces and reports them on specific topics. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `dynamic_joint_states` [*control_msgs/msg/DynamicJointState*] - provides information about the state of various movable joints in a robotic system. -- `joint_states` [*sensor_msgs/msg/JointState*] - provides information about the state of various joints in a robotic system. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) - -[//]: # (ROS_API_NODE_START) - -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) - -[//]: # (ROS_API_NODE_NAME_START) - -### drive_controller - -[//]: # (ROS_API_NODE_NAME_END) - -[//]: # (ROS_API_NODE_DESCRIPTION_START) - -External node type:*[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller)*. - -Controller which manages mobile robots with a differential drive. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it. - -[//]: # (ROS_API_NODE_DESCRIPTION_END) - -#### Subscribers - -[//]: # (ROS_API_NODE_SUBSCRIBERS_START) - -- `cmd_vel` [*geometry_msgs/msg/Twist*]: command linear and angular velocity values. - -[//]: # (ROS_API_NODE_SUBSCRIBERS_END) - -#### Publishers - -[//]: # (ROS_API_NODE_PUBLISHERS_START) +## Launch Files -- `/tf` [*tf2_msgs/msg/TFMessage*]: tf tree. Published only if `enable_odom_tf=true` -- `odometry/wheels` [*nav_msgs/msg/Odometry*]: odometry data from wheel encoders. +- `controller.launch.py`: Establishes communication with the hardware by loading the robot's URDF with plugins and configures the controllers to exchange information between the engine driver and the IMU. -[//]: # (ROS_API_NODE_PUBLISHERS_END) -[//]: # (ROS_API_NODE_END) +## Configuration Files -[//]: # (ROS_API_PACKAGE_END) +- [`WH01_controller.yaml`](./config/WH01_controller.yaml): Configures `imu_broadcaster`, `joint_state_broadcaster` and `drive_controller` controllers for default WH01 wheels. +- [`WH02_controller.yaml`](./config/WH02_controller.yaml): Configures `imu_broadcaster`, `joint_state_broadcaster` and `drive_controller` controllers for mecanum WH02 wheels. +- [`WH04_controller.yaml`](./config/WH04_controller.yaml): Configures `imu_broadcaster`, `joint_state_broadcaster` and `drive_controller` controllers for small pneumatic WH04 wheels. diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py index 8a34ef7a7..92c9b7a63 100644 --- a/panther_controller/launch/controller.launch.py +++ b/panther_controller/launch/controller.launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): ), description=( "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/_controller.yaml'. You can also specify" + " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify" " the path to your custom controller configuration file here. " ), ) @@ -93,7 +93,7 @@ def generate_launch_description(): "Whether to launch the robot_state_publisher node." "When set to False, users should publish their own robot description." ), - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) wheel_config_path = LaunchConfiguration("wheel_config_path") @@ -101,7 +101,7 @@ def generate_launch_description(): "use_sim", default_value="False", description="Whether simulation is used", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) declare_wheel_config_path_arg = DeclareLaunchArgument( @@ -115,7 +115,7 @@ def generate_launch_description(): ), description=( "Path to wheel configuration file. By default, it is located in " - "'panther_description/config/.yaml'. You can also specify the path " + "'panther_description/config/{wheel_type}.yaml'. You can also specify the path " "to your custom wheel configuration file here. " ), ) diff --git a/panther_description/CMakeLists.txt b/panther_description/CMakeLists.txt index 741a0a391..87ff28f03 100644 --- a/panther_description/CMakeLists.txt +++ b/panther_description/CMakeLists.txt @@ -7,5 +7,6 @@ install(DIRECTORY config launch meshes rviz urdf DESTINATION share/${PROJECT_NAME}) ament_environment_hooks( - "${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in") + "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in") + ament_package() diff --git a/panther_description/README.md b/panther_description/README.md index 81a4197bc..6c15c4c40 100644 --- a/panther_description/README.md +++ b/panther_description/README.md @@ -1,3 +1,14 @@ # panther_description The package contains URDF files responsible for creating a representation of the robot by specifying the relationships and types of connections (joints) between individual links. It also contains information about the robot's mesh. + +## Launch Files + +- `load_urdf.launch.py` - loads the robot's URDF and creates simple bindings to display moving joints. + +## Configuration Files + +- [`components.yaml`](./config/components.yaml): Allows you to quickly add visualization of sensors, TF connections and simulate their behavior in the simulator. +- [`WH01.yaml`](./config/WH01.yaml): Description of physical and visual parameters for the wheel WH01. +- [`WH02.yaml`](./config/WH02.yaml): Description of physical and visual parameters for the wheel WH02. +- [`WH04.yaml`](./config/WH04.yaml): Description of physical and visual parameters for the wheel WH04. diff --git a/panther_description/env-hooks/panther_description.sh.in b/panther_description/hooks/panther_description.sh.in similarity index 100% rename from panther_description/env-hooks/panther_description.sh.in rename to panther_description/hooks/panther_description.sh.in diff --git a/panther_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py index d8b89e84c..13a0b83d9 100644 --- a/panther_description/launch/load_urdf.launch.py +++ b/panther_description/launch/load_urdf.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): "add_wheel_joints", default_value="True", description="Flag enabling joint_state_publisher to publish information about the wheel position. Should be false when there is a controller that sends this information.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) battery_config_path = LaunchConfiguration("battery_config_path") @@ -78,7 +78,7 @@ def generate_launch_description(): ), description=( "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/_controller.yaml'. You can also specify" + " 'panther_controller/config/{wheel_type}_controller.yaml'. You can also specify" " the path to your custom controller configuration file here. " ), ) @@ -95,7 +95,7 @@ def generate_launch_description(): "use_sim", default_value="False", description="Whether simulation is used.", - choices=["True", "False"], + choices=["True", "true", "False", "false"], ) declare_wheel_config_path_arg = DeclareLaunchArgument( @@ -109,7 +109,7 @@ def generate_launch_description(): ), description=( "Path to wheel configuration file. By default, it is located in " - "'panther_description/config/.yaml'. You can also specify the path " + "'panther_description/config/{wheel_type}.yaml'. You can also specify the path " "to your custom wheel configuration file here. " ), ) diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro index 705bd760a..781e097d1 100644 --- a/panther_description/urdf/panther_macro.urdf.xacro +++ b/panther_description/urdf/panther_macro.urdf.xacro @@ -63,7 +63,7 @@ panther_gazebo/PantherSystem - 20 + 100 true diff --git a/panther_diagnostics/CMakeLists.txt b/panther_diagnostics/CMakeLists.txt index 6c73bb2f4..c94d1c1e4 100644 --- a/panther_diagnostics/CMakeLists.txt +++ b/panther_diagnostics/CMakeLists.txt @@ -15,25 +15,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_INCLUDE_DEPENDS +set(PACKAGE_DEPENDENCIES ament_cmake - rclcpp - panther_msgs - panther_utils - diagnostic_updater diagnostic_msgs - std_msgs + diagnostic_updater generate_parameter_library - PkgConfig) + panther_msgs + panther_utils + PkgConfig + rclcpp + std_msgs) -foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) endforeach() include_directories(include) set(CPPUPROFILE_PREFIX ${CMAKE_BINARY_DIR}/ep_cppuprofile/src/ep_cppuprofile) set(ENV{PKG_CONFIG_PATH} "${CPPUPROFILE_PREFIX}/lib:$ENV{PKG_CONFIG_PATH}") + pkg_check_modules(CPPUPROFILE REQUIRED IMPORTED_TARGET cppuprofile) generate_parameter_library(system_status_parameters @@ -42,26 +43,25 @@ generate_parameter_library(system_status_parameters add_executable(system_status_node src/main.cpp src/system_status_node.cpp) target_include_directories(system_status_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include) - -ament_target_dependencies(system_status_node ${PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(system_status_node ${PACKAGE_DEPENDENCIES}) target_link_libraries(system_status_node system_status_parameters PkgConfig::CPPUPROFILE) -install(DIRECTORY include/ DESTINATION include/) -install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) - install(TARGETS system_status_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_system_status test/test_system_status_node.cpp src/system_status_node.cpp) - target_include_directories(${PROJECT_NAME}_test_system_status - PUBLIC ${CMAKE_INSTALL_PREFIX}/include) - + target_include_directories( + ${PROJECT_NAME}_test_system_status + PUBLIC $ + $ ${CMAKE_INSTALL_PREFIX}/include) ament_target_dependencies(${PROJECT_NAME}_test_system_status - ament_cmake_gtest ${PACKAGE_INCLUDE_DEPENDS}) + ${PACKAGE_DEPENDENCIES}) target_link_libraries(${PROJECT_NAME}_test_system_status system_status_parameters PkgConfig::CPPUPROFILE) diff --git a/panther_diagnostics/README.md b/panther_diagnostics/README.md index 0c85ba3ec..e4d412464 100644 --- a/panther_diagnostics/README.md +++ b/panther_diagnostics/README.md @@ -1,50 +1,31 @@ -[//]: # (ROS_API_PACKAGE_START) -[//]: # (ROS_API_PACKAGE_NAME_START) - # panther_diagnostics -[//]: # (ROS_API_PACKAGE_NAME_END) -[//]: # (ROS_API_PACKAGE_DESCRIPTION_START) - Package containing nodes monitoring and publishing the Built-in Computer status of Husarion Panther robot. -[//]: # (ROS_API_PACKAGE_DESCRIPTION_END) +## Launch Files -## ROS Nodes +- `system_status.launch.py`: Launch a node that analyzes the state of the most important components in the robot -[//]: # (ROS_API_NODE_START) -[//]: # (ROS_API_NODE_COMPATIBLE_1_0) -[//]: # (ROS_API_NODE_COMPATIBLE_1_2) -[//]: # (ROS_API_NODE_NAME_START) +## Configuration Files -### system_monitor +- [`system_status_parameters.yaml`](./config/system_status_parameters.yaml): Defines parameters for `system_status_node`. -[//]: # (ROS_API_NODE_NAME_END) -[//]: # (ROS_API_NODE_DESCRIPTION_START) +## ROS Nodes -Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and CPU temperature. +- [`system_status_node`](#system_status_node): Publishes system state of the Built-in Computer such as CPU usage, RAM usage, disk usage and CPU temperature. -[//]: # (ROS_API_NODE_DESCRIPTION_END) +### system_status_node #### Publishes -[//]: # (ROS_API_NODE_PUBLISHERS_START) - -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: system status diagnostic messages. -- `system_status` [*panther_msgs/SystemStatus*]: system status statistics. - -[//]: # (ROS_API_NODE_PUBLISHERS_END) +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System status diagnostic messages. +- `system_status` [*panther_msgs/SystemStatus*]: State of the system, including Built-in Computer's CPU temperature and load. #### Parameters -[//]: # (ROS_API_NODE_PARAMETERS_START) - -- `~frame_id` [*string*, default: **build_in_computer**]: Frame where computer is located. +- `~frame_id` [*string*, default: **built_in_computer**]: Frame where computer is located. - `~publish_rate` [*double*, default: **0.25**]: System status publish rate in seconds. - `~disk_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for disk usage warning in percentage. - `~memory_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for memory usage warning in percentage. - `~cpu_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for CPU usage warning in percentage. - `~cpu_temperature_warn_threshold` [*float*, default: **80.0**]: Threshold for CPU temperature warning in degrees Celsius. - -[//]: # (ROS_API_NODE_PARAMETERS_END) -[//]: # (ROS_API_NODE_END) diff --git a/panther_diagnostics/cmake/SuperBuild.cmake b/panther_diagnostics/cmake/SuperBuild.cmake index 1b2c25333..4370b4cdd 100644 --- a/panther_diagnostics/cmake/SuperBuild.cmake +++ b/panther_diagnostics/cmake/SuperBuild.cmake @@ -20,11 +20,11 @@ list(APPEND DEPENDENCIES ep_cppuprofile) ExternalProject_Add( ep_cppuprofile + INSTALL_DIR ${CMAKE_INSTALL_PREFIX} GIT_REPOSITORY https://github.com/Orange-OpenSource/cppuprofile.git GIT_TAG 1.1.1 PREFIX ${CMAKE_CURRENT_BINARY_DIR}/ep_cppuprofile BUILD_COMMAND $(MAKE) -C - INSTALL_DIR ${CMAKE_INSTALL_PREFIX} INSTALL_COMMAND make install INSTALL_PREFIX= CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} BUILD_IN_SOURCE 1) diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt index 6287b2684..8e8851c79 100644 --- a/panther_gazebo/CMakeLists.txt +++ b/panther_gazebo/CMakeLists.txt @@ -1,54 +1,41 @@ cmake_minimum_required(VERSION 3.10.2) project(panther_gazebo) -# Default to C11 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 11) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -# find_package(ament_index_cpp REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(ignition-common4 REQUIRED) -find_package(ignition-gazebo6 REQUIRED) -find_package(ignition-msgs8 REQUIRED) -find_package(ignition-plugin1 REQUIRED) -find_package(ignition-transport11 QUIET REQUIRED) -find_package(ign_ros2_control REQUIRED) -find_package(pluginlib REQUIRED) -find_package(panther_utils REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(yaml-cpp REQUIRED) - -include_directories(include) - -set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) -set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) -set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) -set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) -set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) +set(PACKAGE_DEPENDENCIES + ament_cmake + hardware_interface + ignition-common4 + ignition-gazebo6 + ignition-msgs8 + ignition-plugin1 + ignition-transport11 + ign_ros2_control + pluginlib + panther_utils + rclcpp + std_msgs + std_srvs + yaml-cpp) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +find_package(Qt5 REQUIRED COMPONENTS Core Quick QuickControls2) + +include_directories(include ${Qt5Core_INCLUDE_DIRS} ${Qt5Qml_INCLUDE_DIRS} + ${Qt5Quick_INCLUDE_DIRS} ${Qt5QuickControls2_INCLUDE_DIRS}) add_executable(gz_led_strip_manager src/main.cpp src/gz_led_strip_manager.cpp src/gz_led_strip.cpp) -target_link_libraries( - gz_led_strip_manager ignition-transport${IGN_TRANSPORT_VER}::core - ignition-msgs${IGN_MSGS_VER} - ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} yaml-cpp) ament_target_dependencies(gz_led_strip_manager panther_utils) - -install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) +target_link_libraries( + gz_led_strip_manager ignition-transport11::core ignition-msgs8 + ignition-common4::ignition-common4 yaml-cpp) add_library(panther_hardware_plugins SHARED src/gz_panther_system.cpp) ament_target_dependencies( @@ -59,18 +46,34 @@ ament_target_dependencies( std_msgs std_srvs ign_ros2_control) -target_link_libraries(panther_hardware_plugins - ignition-gazebo${IGN_GAZEBO_VER}::core) +target_link_libraries(panther_hardware_plugins ignition-gazebo6::core) + +set(CMAKE_AUTOMOC ON) +qt5_add_resources(resources_rcc src/gui/Estop.qrc) + +add_library(Estop SHARED include/panther_gazebo/gui/Estop.hpp src/gui/Estop.cpp + ${resources_rcc}) +ament_target_dependencies(Estop ignition-common4 ignition-gazebo6 + ignition-plugin1 rclcpp std_srvs) +target_link_libraries(Estop ${Qt5Core_LIBRARIES} ${Qt5Qml_LIBRARIES} + ${Qt5Quick_LIBRARIES} ${Qt5QuickControls2_LIBRARIES}) + +install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME}) install( TARGETS panther_hardware_plugins - ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) + ARCHIVE DESTINATION lib) -install(DIRECTORY include/ DESTINATION include) +install( + TARGETS Estop + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) + +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) -# Testing and linting if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -81,5 +84,7 @@ ament_export_include_directories(include) pluginlib_export_plugin_description_file(ign_ros2_control panther_hardware_plugins.xml) -# Setup the project +ament_environment_hooks( + "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in") + ament_package() diff --git a/panther_gazebo/CONFIGURATION.md b/panther_gazebo/CONFIGURATION.md new file mode 100644 index 000000000..8a8c4c198 --- /dev/null +++ b/panther_gazebo/CONFIGURATION.md @@ -0,0 +1,61 @@ +# panther_gazebo + +## Use of GPS in Simulation + +The NavSat system is utilized to publish the Panther robot's position within the Gazebo world. It manages navigation satellite sensors, such as GPS, which report position and velocity in spherical coordinates (latitude/longitude) through Ignition Transport. + +The NavSat sensors requires the spherical coordinates of the world origin to be configured. This configuration can be accomplished, for instance, by employing the `` tag within the world's SDF or by utilizing the Ignition `/world/world_name/set_spherical_coordinates` service. + +To obtain GPS data in Ignition, follow these steps: + +- Include the [ANT02](https://github.com/husarion/ros_components_description/blob/ros2/urdf/external_antenna.urdf.xacro) by adding the following lines to your [components.yaml](../panther_description/config/components.yaml) file inside the `components` list: + +```yaml + - type: ANT02 + parent_link: cover_link + xyz: 0.185 -0.12 0.0 + rpy: 0.0 0.0 3.14 + device_namespace: gps +``` + +- Add the following tag to your world's SDF file and specify this file using the `world` parameter (the default `husarion_world.sdf` file already includes this tag): + +```xml + + EARTH_WGS84 + ENU + 53.1978 + 18.3732 + 0 + 0 + +``` + +## Linear Battery Plugin + +It is possible to simulate the battery operation of the Panther robot. By default, this feature is disabled, but you can enable it by setting the `simulate_discharging` parameter to `true` in the `battery_plugin_config.yaml` file or in the file pointed to by the `battery_config_path` parameter. Below, you will find the plugin parameters that enable battery simulation. + +- `simulate_discharging` [*bool*, default: **false**]: Enables battery simulation. If set to `true`, the battery will discharge **at a constant rate** (regardless of joint torque), and if it depletes completely, the robot will stop moving. When set to `false`, the battery will not discharge, but the battery status information will still be published on the `battery/battery_status` topic. +- `initial_charge_percentage` [*float*, default: **70.0**]: Sets the initial charge percentage of the battery. +- `charging_time` [*float*, default: **6.0**]: Specifies the charging time for the battery in hours. +- `power_load` [*float*, default: **120.0**]: Represents the average power load during normal operation **[W]** and is initially set to 120.0 W. With the default power load of 120.0 W, the robot can operate for up to 8 hours. When the `simulate_discharging` parameter is set to `false`, this value defaults to 0.0 W. Users are encouraged to customize this value to match their specific requirements. For more information on Panther power consumption, please refer to [Panther Battery & Charging Documentation](https://husarion.com/manuals/panther/#battery--charging). + +> [!NOTE] +> +> The battery model is quite simple and involves significant simplifications. As a result, the battery discharge rate observed on the physical robot may differ from the model's predictions. + +### Charging Process + +Unfortunately, there is no straightforward way to exchange `LinearBatteryPlugin` services between ROS and Gazebo Transport, so you need to use the `ign` commands. As a result, the method of charging differs between the real and simulated robot. + +You can start the charging process by calling the Ignition service: + +```bash +ign service --service /model/panther/battery/battery/recharge/start --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 +``` + +and stop it using: + +```bash +ign service --service /model/panther/battery/battery/recharge/stop --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 +``` diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md index 1c7783994..ed74555ce 100644 --- a/panther_gazebo/README.md +++ b/panther_gazebo/README.md @@ -2,97 +2,51 @@ The package contains a launch file and source files used to run the robot simulation in Gazebo. The simulator tries to reproduce the behavior of a real robot as much as possible, including the provision of an analogous ROS_API. -## Available Launch File +## Launch Files -- `spawn_robot.launch.py` - is responsible for spawning the robot in the simulator -- `simulate_robot.launch.py` - is responsible for giving birth to the robot and simulating its physical behavior, such as driving, displaying data, etc. -- `simulate_multiple_robots.launch.py` - similar to the above with logic allowing you to quickly add a swarm of robots -- **`simulation.launch.py`** - a target file that runs the gazebo simulator and adds and simulates the robot's behavior in accordance with the given arguments. +- `spawn_robot.launch.py`: Responsible for spawning the robot in the simulator. +- `simulate_robot.launch.py`: Responsible for giving birth to the robot and simulating its physical behavior, such as driving, displaying data, etc. +- `simulate_multiple_robots.launch.py`: Similar to the above with logic allowing you to quickly add a swarm of robots. +- **`simulation.launch.py`**: A target file that runs the gazebo simulator that adds and simulates the robot's behavior in accordance with the given arguments. -## Usage +## Configuration Files -The recommended method for launching the simulation is by utilizing the [simulation.launch.py](https://github.com/husarion/panther_ros/panther_gazebo/launch/simulation.launch.py) file. Below, you will find launch arguments that enable simulation configuration. You can also launch more robots using `spawn.launch.py` ​​after the system has been started. +- [`battery_plugin_config.yaml`](./config/battery_plugin_config.yaml): Simulated LinearBatteryPlugin configuration. +- [`gz_bridge.yaml`](./config/gz_bridge.yaml): Specify data to exchange between ROS and Gazebo simulation. +- [`led_strips.yaml`](./config/led_strips.yaml): Configure properties of led strips in simulation to animate lights. +- [`teleop_with_estop.config`](./config/teleop_with_estop.config): Gazebo layout configuration file, which add E-Stop and Teleop widgets. -### Launch Arguments +## ROS Nodes -- `add_world_transform` [*bool*, default: **False**]: Adds a world frame that connects the tf trees of individual robots (useful when running multiple robots). -- `battery_config_path` [*string*, default: **panther_gazebo/config/battery_plugin_config.yaml**]: Path to the Ignition `LinearBatteryPlugin` configuration file. This configuration is intended for use in simulations only. For more information on how to configure this plugin, please refer to the [Linear Battery Plugin](#linear-battery-plugin) section. -- `controller_config_path` [*string*, default: **panther_controller/config/_controller.yaml**]: Path to the controller configuration file. If you want to use a custom configuration, you can specify the path to your custom controller configuration file here. -- `gz_bridge_config_path` [*string*, default: **panther_gazebo/config/gz_bridge.yaml**]: Path to the `parameter_bridge` configuration file. For detailed information on configuring the `parameter_bridge`, please refer to this [example](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge#example-5-configuring-the-bridge-via-yaml). -- `gz_log_level` [*[0-4]*, default: **1**]: Adjust the level of console output. -- `headless_mode` [*bool*, default: **False**]: Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the amount of calculations. -- `x` [*float*, default: **5.0**]: spawn position **[m]** of the robot in the world in **X** direction. -- `y` [*float*, default: **-5.0**]: spawn position **[m]** of the robot in the world in **Y** direction. -- `z` [*float*, default: **0.2**]: spawn position **[m]** of the robot in the world in **Z** direction. -- `roll` [*float*, default: **0.0**]: spawn roll angle **[rad]** of the robot in the world. -- `pitch` [*float*, default: **0.0**]: spawn pitch angle **[rad]** of the robot in the world. -- `yaw` [*float*, default: **0.0**]: spawn yaw angle **[rad]** of the robot in the world. -- `publish_robot_state` [*bool*, default: **true**]: Whether to launch the robot_state_publisher node. When set to `false`, users should publish their own robot description. -- `robots` [*yaml style*, default: **""**]: The list of the robots spawned in the simulation e.g. robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}'" -- `wheel_config_path` [*string*, default: **panther_description/config/.yaml**]: Path to the wheel configuration file. If you want to use a custom configuration, you can specify the path to your custom wheel configuration file here. Please refer to the `wheel_type` parameter description for more information. -- `wheel_type` [*string*, default: **WH01**]: Specify the type of wheel. If you select a value from the provided options (`WH01`, `WH02`, `WH04`), you can disregard the `wheel_config_path` and `controller_config_path` parameters. If you have custom wheels, set this parameter to `CUSTOM` and provide the necessary configurations. -- `world` [*string*, default: **-r /worlds/husarion_world.sdf**]: path to Gazebo world file used for simulation. +### Estop -### Changing Wheel Type +`Estop` is a Gazebo GUI plugin responsible for easy and convenient changing of the robot's E-stop state. -It is possible to change Panther wheels model in simulation. All you need to do is to point to new wheel and controller configuration files using `wheel_config_path` and `controller_config_path` parameters. These files should be based on the default ones, i.e., [WH01_controller.yaml](https://github.com/husarion/panther_ros/panther_controller/config/WH01_controller.yaml) and [WH01.yaml](https://github.com/husarion/panther_ros/panther_description/config/WH01.yaml). +#### Publishers -### Linear Battery Plugin +- `gz_ros2_control/e_stop` [*std_msgs/Bool*]: Current E-stop state. -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. +#### Service Servers -- `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). +- `gz_ros2_control/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop. +- `gz_ros2_control/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop. -> **Note** -> -> The `percentage` field has a range of 0.0-100.0. This differs from the real functioning of Panther, where, in accordance with the BatteryState message definition, the value is within the range of 0.0-1.0. +### PantherSystem -#### Charging Process +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. -Unfortunately, there is no straightforward way to exchange `LinearBatteryPlugin` services between ROS and Gazebo Transport, so you need to use the `ign` commands. As a result, the method of charging differs between the real and simulated robot. +#### Publishers -You can start the charging process by calling the Ignition service: +- `hardware/e_stop` [*std_msgs/Bool*]: Current E-stop state. -```bash -ign service --service /model/panther/battery/panther_battery/recharge/start --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 -``` +#### Service Servers -and stop it using: +- `hardware/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop. +- `hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop. -```bash -ign service --service /model/panther/battery/panther_battery/recharge/stop --reqtype ignition.msgs.Boolean --reptype ignition.msgs.Empty --req '' --timeout 0 -``` +#### Parameters -### Use of GPS in Simulation +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)). -The NavSat system is utilized to publish the Panther robot's position within the Gazebo world. It manages navigation satellite sensors, such as GPS, which report position and velocity in spherical coordinates (latitude/longitude) through Ignition Transport. - -The NavSat sensors requires the spherical coordinates of the world origin to be configured. This configuration can be accomplished, for instance, by employing the `` tag within the world's SDF or by utilizing the Ignition `/world/world_name/set_spherical_coordinates` service. - -To obtain GPS data in Ignition, follow these steps: - -- Include the [ANT02](https://github.com/husarion/ros_components_description/blob/ros2/urdf/external_antenna.urdf.xacro) by adding the following lines to your [components.yaml](https://github.com/husarion/panther_ros/blob/ros2/panther_description/config/components.yaml) file inside the `components` list: - -```yaml - - type: ANT02 - parent_link: cover_link - xyz: 0.185 -0.12 0.0 - rpy: 0.0 0.0 3.14 - device_namespace: gps -``` - -- Add the following tag to your world's SDF file and specify this file using the `world` parameter (the default `husarion_world.sdf` file already includes this tag): - -```xml - - EARTH_WGS84 - ENU - 53.1978 - 18.3732 - 0 - 0 - -``` +- `e_stop_publish_frequency` [*float*, default: **100**]: Frequency of publication of e-stop status information. +- `e_stop_initial_state` [*bool*, default: **true**]: Initial state of E-stop. diff --git a/panther_gazebo/config/gz_bridge.yaml b/panther_gazebo/config/gz_bridge.yaml index 5f8fa9209..8b6c2328c 100644 --- a/panther_gazebo/config/gz_bridge.yaml +++ b/panther_gazebo/config/gz_bridge.yaml @@ -17,14 +17,12 @@ gz_type_name: ignition.msgs.Twist direction: GZ_TO_ROS -- ros_topic_name: lights/channel_1_frame - gz_topic_name: lights/channel_1_frame +- topic_name: lights/channel_1_frame ros_type_name: sensor_msgs/msg/Image gz_type_name: ignition.msgs.Image direction: ROS_TO_GZ -- ros_topic_name: lights/channel_2_frame - gz_topic_name: lights/channel_2_frame +- topic_name: lights/channel_2_frame ros_type_name: sensor_msgs/msg/Image gz_type_name: ignition.msgs.Image direction: ROS_TO_GZ diff --git a/panther_gazebo/config/teleop_with_estop.config b/panther_gazebo/config/teleop_with_estop.config new file mode 100644 index 000000000..05046bec5 --- /dev/null +++ b/panther_gazebo/config/teleop_with_estop.config @@ -0,0 +1,1266 @@ + + + + -1 + -1 + + 1850 + 1016 +