Ros2 api reorganization

.github/workflows/release-repository.yaml

@@ -62,7 +62,7 @@ jobs:
 
       - name: Grant permissions # Required permission grant after at-wat/catkin-release-action@v1 action
         if: ${{ fromJSON(inputs.automatic_mode) == true }}
-        run: | 
+        run: |
           sudo chmod -R ugo+rwX .
 
       - name: Checkout to main branch

panther_battery/README.md

@@ -17,7 +17,7 @@ Package containing nodes monitoring and publishing the internal battery state of
 [//]: # (ROS_API_NODE_COMPATIBLE_1_2)
 [//]: # (ROS_API_NODE_NAME_START)
 
-### battery_node
+### battery_driver
 
 [//]: # (ROS_API_NODE_NAME_END)
 [//]: # (ROS_API_NODE_DESCRIPTION_START)
@@ -30,19 +30,20 @@ Publishes battery state read from ADC unit for Panther version 1.2 and above, or
 
 [//]: # (ROS_API_NODE_PUBLISHERS_START)
 
+- `_battery/battery_1_status_raw` [*sensor_msgs/BatteryState*]: first battery raw state.
+- `_battery/battery_2_status_raw` [*sensor_msgs/BatteryState*]: second battery raw state. Published if second battery detected.
+- `battery/battery_status` [*sensor_msgs/BatteryState*]: mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published.
+- `battery/charging_status` [*panther_msgs/ChargingStatus*]: battery charging status.
 - `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: battery diagnostic messages.
-- `battery` [*sensor_msgs/BatteryState*]: mean values of both batteries if Panther has two batteries. Otherwise, the state of the single battery will be published.
-- `battery_1_raw` [*sensor_msgs/BatteryState*]: first battery raw state.
-- `battery_2_raw` [*sensor_msgs/BatteryState*]: second battery raw state. Published if second battery detected.
-- `charging_status` [*panther_msgs/ChargingStatus*]: battery charging status.
 
 [//]: # (ROS_API_NODE_PUBLISHERS_END)
 
 #### Subscribes
 
 [//]: # (ROS_API_NODE_SUBSCRIBERS_START)
 
-- `driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
+- `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)
 
@@ -52,6 +53,7 @@ Publishes battery state read from ADC unit for Panther version 1.2 and above, or
 
 - `~/adc/device0` [*string*, default: **/dev/adc0**]: ADC nr 0 IIO device. Used with Panther version 1.2 and above.
 - `~/adc/device1` [*string*, default: **/dev/adc1**]: ADC nr 1 IIO device. Used with Panther version 1.2 and above.
+- `~/adc/path` [*string*, default: **/sys/bus/iio/devices/**]: path of ADC devices mount.
 - `~/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.

panther_battery/include/panther_battery/battery_node.hpp

@@ -43,9 +43,10 @@ class BatteryNode : public rclcpp::Node
   void Initialize();
   void InitializeWithADCBattery();
   void InitializeWithRoboteqBattery();
-  std::string GetADCDevicePath(const std::string & adc_device_path) const;
+  std::string GetADCDevicePath(const std::string & adc_device_name) const;
 
   static constexpr int kADCCurrentOffset = 625;
+
   DriverStateMsg::SharedPtr driver_state_;
 
   std::shared_ptr<ADCDataReader> adc0_reader_;

panther_battery/launch/battery.launch.py

@@ -29,10 +29,11 @@ def generate_launch_description():
     )
 
     panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
-    battery_node = Node(
+
+    battery_driver_node = Node(
         package="panther_battery",
         executable="battery_node",
-        name="battery_node",
+        name="battery_driver",
         parameters=[{"panther_version": panther_version}],
         namespace=namespace,
         remappings=[("/diagnostics", "diagnostics")],
@@ -41,7 +42,7 @@ def generate_launch_description():
 
     actions = [
         declare_namespace_arg,
-        battery_node,
+        battery_driver_node,
     ]
 
     return LaunchDescription(actions)

panther_battery/src/battery_node.cpp

@@ -80,6 +80,7 @@ void BatteryNode::InitializeWithADCBattery()
 
   this->declare_parameter<std::string>("adc/device0", "/dev/adc0");
   this->declare_parameter<std::string>("adc/device1", "/dev/adc1");
+  this->declare_parameter<std::string>("adc/path", "/sys/bus/iio/devices");
   this->declare_parameter<int>("adc/ma_window_len/temp", 10);
   this->declare_parameter<int>("adc/ma_window_len/charge", 10);
 
@@ -143,7 +144,7 @@ void BatteryNode::InitializeWithRoboteqBattery()
   };
 
   driver_state_sub_ = this->create_subscription<DriverStateMsg>(
-    "driver/motor_controllers_state", 5,
+    "hardware/motor_controllers_state", 5,
     [&](const DriverStateMsg::SharedPtr msg) { driver_state_ = msg; });
 
   battery_1_ = std::make_shared<RoboteqBattery>([&]() { return driver_state_; }, battery_params);
@@ -163,17 +164,18 @@ void BatteryNode::BatteryPubTimerCB()
   battery_publisher_->Publish();
 }
 
-std::string BatteryNode::GetADCDevicePath(const std::string & adc_device_path) const
+std::string BatteryNode::GetADCDevicePath(const std::string & adc_device_name) const
 {
-  std::string adc_device_name;
+  const std::string adc_path = this->get_parameter("adc/path").as_string();
+  std::filesystem::path adc_device;
 
-  if (std::filesystem::is_symlink(adc_device_path)) {
-    adc_device_name = std::filesystem::read_symlink(adc_device_path).string();
+  if (std::filesystem::is_symlink(adc_device_name)) {
+    adc_device = std::filesystem::read_symlink(adc_device_name);
   } else {
-    adc_device_name = std::filesystem::path(adc_device_path).filename().string();
+    adc_device = std::filesystem::path(adc_device_name).filename();
   }
 
-  return std::string("/sys/bus/iio/devices/") + adc_device_name;
+  return (adc_path / adc_device).string();
 }
 
 }  // namespace panther_battery

panther_battery/src/dual_battery_publisher.cpp

@@ -39,10 +39,10 @@ DualBatteryPublisher::DualBatteryPublisher(
   battery_1_(std::move(battery_1)),
   battery_2_(std::move(battery_2))
 {
-  battery_pub_ = node->create_publisher<BatteryStateMsg>("battery", 5);
-  battery_1_pub_ = node->create_publisher<BatteryStateMsg>("battery_1_raw", 5);
-  battery_2_pub_ = node->create_publisher<BatteryStateMsg>("battery_2_raw", 5);
-  charging_status_pub_ = node->create_publisher<ChargingStatusMsg>("charging_status", 5);
+  battery_pub_ = node->create_publisher<BatteryStateMsg>("battery/battery_status", 5);
+  battery_1_pub_ = node->create_publisher<BatteryStateMsg>("_battery/battery_1_status_raw", 5);
+  battery_2_pub_ = node->create_publisher<BatteryStateMsg>("_battery/battery_2_status_raw", 5);
+  charging_status_pub_ = node->create_publisher<ChargingStatusMsg>("battery/charging_status", 5);
 }
 
 void DualBatteryPublisher::Update()

panther_battery/src/main.cpp

@@ -24,15 +24,15 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
 
-  auto battery_node = std::make_shared<panther_battery::BatteryNode>("battery_node");
+  auto battery_node = std::make_shared<panther_battery::BatteryNode>("battery_driver");
 
   try {
     rclcpp::spin(battery_node);
   } catch (const std::runtime_error & e) {
-    std::cerr << "[battery_node] Caught exception: " << e.what() << std::endl;
+    std::cerr << "[battery_driver] Caught exception: " << e.what() << std::endl;
   }
 
-  std::cout << "[battery_node] Shutting down" << std::endl;
+  std::cout << "[battery_driver] Shutting down" << std::endl;
   rclcpp::shutdown();
   return 0;
 }

panther_battery/src/single_battery_publisher.cpp

@@ -35,9 +35,9 @@ SingleBatteryPublisher::SingleBatteryPublisher(
   const std::shared_ptr<Battery> & battery)
 : BatteryPublisher(std::move(node), std::move(diagnostic_updater)), battery_(std::move(battery))
 {
-  battery_pub_ = node->create_publisher<BatteryStateMsg>("battery", 5);
-  battery_1_pub_ = node->create_publisher<BatteryStateMsg>("battery_1_raw", 5);
-  charging_status_pub_ = node->create_publisher<ChargingStatusMsg>("charging_status", 5);
+  battery_pub_ = node->create_publisher<BatteryStateMsg>("battery/battery_status", 5);
+  battery_1_pub_ = node->create_publisher<BatteryStateMsg>("_battery/battery_1_status_raw", 5);
+  charging_status_pub_ = node->create_publisher<ChargingStatusMsg>("battery/charging_status", 5);
 }
 
 void SingleBatteryPublisher::Update()

panther_battery/test/include/test_battery_node.hpp

@@ -47,6 +47,9 @@ class TestBatteryNode : public testing::Test
   template <typename T>
   void WriteNumberToFile(const T number, const std::string & file_path);
 
+  static constexpr char kADCDevice0[] = "adc0";
+  static constexpr char kADCDevice1[] = "adc1";
+
   std::filesystem::path device0_path_;
   std::filesystem::path device1_path_;
   BatteryStateMsg::SharedPtr battery_state_;
@@ -66,11 +69,12 @@ TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_ba
   params.push_back(rclcpp::Parameter("panther_version", panther_version));
 
   if (panther_version >= 1.2 - std::numeric_limits<float>::epsilon()) {
-    device0_path_ = std::filesystem::path(testing::TempDir()) / "device0";
-    device1_path_ = std::filesystem::path(testing::TempDir()) / "device1";
+    device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
+    device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
 
-    params.push_back(rclcpp::Parameter("adc/device0", device0_path_));
-    params.push_back(rclcpp::Parameter("adc/device1", device1_path_));
+    params.push_back(rclcpp::Parameter("adc/device0", kADCDevice0));
+    params.push_back(rclcpp::Parameter("adc/device1", kADCDevice1));
+    params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
 
     // Create the device0 and device1 directories if they do not exist
     std::filesystem::create_directory(device0_path_);
@@ -100,19 +104,22 @@ TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_ba
   rclcpp::NodeOptions options;
   options.parameter_overrides(params);
 
-  battery_node_ = std::make_shared<panther_battery::BatteryNode>("battery_node", options);
+  battery_node_ = std::make_shared<panther_battery::BatteryNode>("battery_driver", options);
 
   battery_sub_ = battery_node_->create_subscription<BatteryStateMsg>(
-    "battery", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_state_ = msg; });
+    "battery/battery_status", 10,
+    [&](const BatteryStateMsg::SharedPtr msg) { battery_state_ = msg; });
   battery_1_sub_ = battery_node_->create_subscription<BatteryStateMsg>(
-    "battery_1_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; });
+    "_battery/battery_1_status_raw", 10,
+    [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; });
   battery_2_sub_ = battery_node_->create_subscription<BatteryStateMsg>(
-    "battery_2_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_2_state_ = msg; });
+    "_battery/battery_2_status_raw", 10,
+    [&](const BatteryStateMsg::SharedPtr msg) { battery_2_state_ = msg; });
 
   io_state_pub_ = battery_node_->create_publisher<IOStateMsg>(
     "hardware/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
   driver_state_pub_ = battery_node_->create_publisher<DriverStateMsg>(
-    "driver/motor_controllers_state", 10);
+    "hardware/motor_controllers_state", 10);
 }
 
 TestBatteryNode::~TestBatteryNode()

panther_battery/test/test_dual_battery_publisher.cpp

@@ -111,11 +111,14 @@ TestDualBatteryPublisher::TestDualBatteryPublisher()
   node_ = std::make_shared<rclcpp::Node>("node");
   diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(node_);
   battery_sub_ = node_->create_subscription<BatteryStateMsg>(
-    "/battery", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_state_ = msg; });
+    "/battery/battery_status", 10,
+    [&](const BatteryStateMsg::SharedPtr msg) { battery_state_ = msg; });
   battery_1_sub_ = node_->create_subscription<BatteryStateMsg>(
-    "/battery_1_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; });
+    "/_battery/battery_1_status_raw", 10,
+    [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; });
   battery_2_sub_ = node_->create_subscription<BatteryStateMsg>(
-    "/battery_2_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_2_state_ = msg; });
+    "/_battery/battery_2_status_raw", 10,
+    [&](const BatteryStateMsg::SharedPtr msg) { battery_2_state_ = msg; });
   battery_publisher_ = std::make_shared<DualBatteryPublisherWrapper>(
     node_, diagnostic_updater_, battery_1_, battery_2_);
 }

panther_battery/test/test_single_battery_publisher.cpp

@@ -56,9 +56,11 @@ TestSingleBatteryPublisher::TestSingleBatteryPublisher()
   node_ = std::make_shared<rclcpp::Node>("node");
   diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(node_);
   battery_sub_ = node_->create_subscription<BatteryStateMsg>(
-    "/battery", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_state_ = msg; });
+    "/battery/battery_status", 10,
+    [&](const BatteryStateMsg::SharedPtr msg) { battery_state_ = msg; });
   battery_1_sub_ = node_->create_subscription<BatteryStateMsg>(
-    "/battery_1_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; });
+    "/_battery/battery_1_status_raw", 10,
+    [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; });
   battery_publisher_ = std::make_shared<panther_battery::SingleBatteryPublisher>(
     node_, diagnostic_updater_, battery_);
 }

panther_bringup/README.md

@@ -12,8 +12,8 @@ The package contains the default configuration and launch files necessary to sta
 
 ## Default Nodes Launched
 
-- `battery_node` [*[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_node` [*[robot_localization/ekf_localization_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).
+- `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
@@ -31,7 +31,7 @@ The package contains the default configuration and launch files necessary to sta
 - `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/<wheel_type arg>.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)
+- `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)
@@ -47,7 +47,7 @@ The package contains the default configuration and launch files necessary to sta
 
 [//]: # (ROS_API_NODE_NAME_START)
 
-### ekf_node
+### ekf_filter
 
 [//]: # (ROS_API_NODE_NAME_END)
 
@@ -82,7 +82,7 @@ Extended Kalman Filter node for more accurate odometry. For more information, re
 
 [//]: # (ROS_API_NODE_SERVICE_SERVERS_START)
 
-- `~/ekf_node/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*.
+- `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)