described parameters | applied codeguides

Signed-off-by: Jakub Delicat <[email protected]>

panther_docking/CMakeLists.txt

@@ -61,7 +61,8 @@ if(BUILD_TESTING)
 
 endif()
 
-add_executable(dock_pose_publisher src/dock_pose_publisher_node.cpp)
+add_executable(dock_pose_publisher src/dock_pose_publisher_node.cpp
+                                   src/dock_pose_publisher_main.cpp)
 ament_target_dependencies(dock_pose_publisher ${PACKAGE_DEPENDENCIES})
 
 install(TARGETS dock_pose_publisher DESTINATION lib/${PROJECT_NAME})

panther_docking/README.md

@@ -53,3 +53,6 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht
 - `panther_charging_dock.filter_coef` [*double*, default: **0.1**]: A key parameter that influences the trade-off between the filter's responsiveness and its smoothness, balancing how quickly it reacts to new pose data pose how much it smooths out fluctuations.
 - `panther_charging_dock.use_wibotic_info` [*bool*, default: **True**]: Whether Wibotic information is used.
 - `panther_charging_dock.wibotic_info_timeout` [*double*, default: **1.5**]: A timeout in seconds to receive wibotic_info.
+- `<dock_name>.apriltag_id` [*int*, default: **0**]: AprilTag ID of a dock.
+- `<dock_name>.dock_frame` [*string*, default: **main_wibotic_receiver_requested_pose_link**]: A frame id to compare with fixed frame if docked.
+- `<dock_name>.pose` [*list*, default: **[0.0, 0.0, 0.0]**]: A pose of a dock on the map. If the simulation is used a dock is spawned in this pose.

panther_docking/config/panther_docking_server.yaml

@@ -29,14 +29,14 @@
       type: panther_charging_dock
       frame: <robot_namespace>/map
       dock_frame: main_wibotic_receiver_requested_pose_link
-      pose: [1.0, 1.5, 1.57] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot
+      pose: [1.0, 1.5, 1.57] # position of the dock on the map. Used also for spawning dock in the simulation.
       apriltag_id: 0
 
     backup:
       type: panther_charging_dock
       frame: <robot_namespace>/map
       dock_frame: backup_wibotic_receiver_requested_pose_link
-      pose: [-1.0, 1.5, 1.57] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot
+      pose: [-1.0, 1.5, 1.57]  # position of the dock on the map. Used also for spawning dock in the simulation.
       apriltag_id: 1
 
     controller:

panther_docking/include/panther_docking/dock_pose_publisher_node.hpp

@@ -0,0 +1,51 @@
+// Copyright (c) 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_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_
+#define PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace panther_docking
+{
+constexpr double kMinimalDetectionDistance = 1.0;
+
+class DockPosePublisherNode : public rclcpp::Node
+{
+public:
+  DockPosePublisherNode(const std::string & name);
+
+private:
+  void publishPose();
+
+  double timeout_;
+  std::string target_frame_;
+  std::vector<std::string> source_frames_;
+  std::string base_frame_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+}  // namespace panther_docking
+
+#endif  // PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_

panther_docking/src/dock_pose_publisher_main.cpp

@@ -0,0 +1,40 @@
+// Copyright (c) 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 <iostream>
+#include <memory>
+#include <stdexcept>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "panther_docking/dock_pose_publisher_node.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  auto dock_pose_publisher_node =
+    std::make_shared<panther_docking::DockPosePublisherNode>("dock_pose_publisher_node");
+
+  try {
+    rclcpp::spin(dock_pose_publisher_node);
+  } catch (const std::runtime_error & e) {
+    std::cerr << "[" << dock_pose_publisher_node->get_name() << "] Caught exception: " << e.what()
+              << std::endl;
+  }
+
+  std::cout << "[" << dock_pose_publisher_node->get_name() << "] Shutting down" << std::endl;
+  rclcpp::shutdown();
+  return 0;
+}

panther_docking/src/dock_pose_publisher_node.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "panther_docking/dock_pose_publisher_node.hpp"
+
 #include <memory>
 #include <string>
 #include <vector>
@@ -23,123 +25,100 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
-constexpr double kMinimalDetectionDistance = 1.0;
-
-class DockPosePublisherNode : public rclcpp::Node
+namespace panther_docking
 {
-public:
-  DockPosePublisherNode() : Node("pose_publisher_node")
-  {
-    declare_parameter("publish_rate", 10.0);
-    declare_parameter("docks", std::vector<std::string>({"main"}));
-    declare_parameter("fixed_frame", "odom");
-    declare_parameter("base_frame", "base_link");
-    declare_parameter("panther_charging_dock.external_detection_timeout", 0.1);
-
-    const auto fixed_frame = get_parameter("fixed_frame").as_string();
-    const auto docks = get_parameter("docks").as_string_array();
-    const auto publish_rate = get_parameter("publish_rate").as_double();
-    const auto publish_period = std::chrono::duration<double>(1.0 / publish_rate);
-
-    timeout_ = get_parameter("panther_charging_dock.external_detection_timeout").as_double() * 0.1;
-    base_frame_ = get_parameter("base_frame").as_string();
-
-    for (const auto & dock : docks) {
-      declare_parameter(dock + ".type", "panther_charging_dock");
-      declare_parameter(dock + ".dock_frame", "main_wibotic_receiver_requested_pose_link");
-
-      const auto dock_type = get_parameter(dock + ".type").as_string();
-      if (dock_type == "panther_charging_dock") {
-        const auto dock_pose_frame_id = get_parameter(dock + ".dock_frame").as_string();
-        RCLCPP_INFO_STREAM(
-          this->get_logger(), "Adding dock " << dock << " with frame " << dock_pose_frame_id);
-        source_frames_.push_back(dock_pose_frame_id);
-      }
+DockPosePublisherNode::DockPosePublisherNode(const std::string & name) : Node(name)
+{
+  declare_parameter("publish_rate", 10.0);
+  declare_parameter("docks", std::vector<std::string>({"main"}));
+  declare_parameter("fixed_frame", "odom");
+  declare_parameter("base_frame", "base_link");
+  declare_parameter("panther_charging_dock.external_detection_timeout", 0.1);
+
+  const auto fixed_frame = get_parameter("fixed_frame").as_string();
+  const auto docks = get_parameter("docks").as_string_array();
+  const auto publish_rate = get_parameter("publish_rate").as_double();
+  const auto publish_period = std::chrono::duration<double>(1.0 / publish_rate);
+
+  timeout_ = get_parameter("panther_charging_dock.external_detection_timeout").as_double() * 0.1;
+  base_frame_ = get_parameter("base_frame").as_string();
+
+  for (const auto & dock : docks) {
+    declare_parameter(dock + ".type", "panther_charging_dock");
+    declare_parameter(dock + ".dock_frame", "main_wibotic_receiver_requested_pose_link");
+
+    const auto dock_type = get_parameter(dock + ".type").as_string();
+    if (dock_type == "panther_charging_dock") {
+      const auto dock_pose_frame_id = get_parameter(dock + ".dock_frame").as_string();
+      RCLCPP_INFO_STREAM(
+        this->get_logger(), "Adding dock " << dock << " with frame " << dock_pose_frame_id);
+      source_frames_.push_back(dock_pose_frame_id);
     }
-    target_frame_ = fixed_frame;
+  }
+  target_frame_ = fixed_frame;
 
-    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
-    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
-    timer_ = this->create_wall_timer(
-      publish_period, std::bind(&DockPosePublisherNode::publishPose, this));
-    pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
-      "docking/dock_pose", 10);
+  timer_ = this->create_wall_timer(
+    publish_period, std::bind(&DockPosePublisherNode::publishPose, this));
+  pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
+    "docking/dock_pose", 10);
 
-    RCLCPP_INFO(this->get_logger(), "DockPosePublisherNode initialized");
-  }
+  RCLCPP_INFO(this->get_logger(), "DockPosePublisherNode initialized");
+}
+
+void DockPosePublisherNode::publishPose()
+{
+  geometry_msgs::msg::PoseStamped pose_msg;
+  pose_msg.header.stamp = this->now();
+  pose_msg.header.frame_id = target_frame_;
 
-private:
-  void publishPose()
-  {
-    geometry_msgs::msg::PoseStamped pose_msg;
-    pose_msg.header.stamp = this->now();
-    pose_msg.header.frame_id = target_frame_;
+  geometry_msgs::msg::TransformStamped closest_dock;
+  geometry_msgs::msg::TransformStamped base_transform_stamped;
 
-    geometry_msgs::msg::TransformStamped closest_dock;
-    geometry_msgs::msg::TransformStamped base_transform_stamped;
+  bool found = false;
 
-    bool found = false;
+  double closest_dist = std::numeric_limits<double>::max();
 
-    double closest_dist = std::numeric_limits<double>::max();
+  try {
+    base_transform_stamped = tf_buffer_->lookupTransform(
+      target_frame_, base_frame_, tf2::TimePointZero);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_DEBUG_STREAM(this->get_logger(), "Could not get transform: " << ex.what());
+    return;
+  }
 
+  for (size_t i = 0; i < source_frames_.size(); ++i) {
+    geometry_msgs::msg::TransformStamped transform_stamped;
     try {
-      base_transform_stamped = tf_buffer_->lookupTransform(
-        target_frame_, base_frame_, tf2::TimePointZero);
+      transform_stamped = tf_buffer_->lookupTransform(
+        target_frame_, source_frames_[i], tf2::TimePointZero, tf2::durationFromSec(timeout_));
     } catch (tf2::TransformException & ex) {
       RCLCPP_DEBUG_STREAM(this->get_logger(), "Could not get transform: " << ex.what());
-      return;
+      continue;
     }
 
-    for (size_t i = 0; i < source_frames_.size(); ++i) {
-      geometry_msgs::msg::TransformStamped transform_stamped;
-      try {
-        transform_stamped = tf_buffer_->lookupTransform(
-          target_frame_, source_frames_[i], tf2::TimePointZero, tf2::durationFromSec(timeout_));
-      } catch (tf2::TransformException & ex) {
-        RCLCPP_DEBUG_STREAM(this->get_logger(), "Could not get transform: " << ex.what());
-        continue;
-      }
-
-      const double dist = std::hypot(
-        transform_stamped.transform.translation.x - base_transform_stamped.transform.translation.x,
-        transform_stamped.transform.translation.y - base_transform_stamped.transform.translation.y);
-
-      if (dist < kMinimalDetectionDistance && dist < closest_dist) {
-        closest_dist = dist;
-        closest_dock = transform_stamped;
-        found = true;
-      }
-    }
+    const double dist = std::hypot(
+      transform_stamped.transform.translation.x - base_transform_stamped.transform.translation.x,
+      transform_stamped.transform.translation.y - base_transform_stamped.transform.translation.y);
 
-    if (!found) {
-      RCLCPP_DEBUG(this->get_logger(), "No dock found");
-      return;
+    if (dist < kMinimalDetectionDistance && dist < closest_dist) {
+      closest_dist = dist;
+      closest_dock = transform_stamped;
+      found = true;
     }
-
-    pose_msg.pose.position.x = closest_dock.transform.translation.x;
-    pose_msg.pose.position.y = closest_dock.transform.translation.y;
-    pose_msg.pose.position.z = 0.0;
-    pose_msg.pose.orientation = closest_dock.transform.rotation;
-    pose_publisher_->publish(pose_msg);
   }
 
-  double timeout_;
-
-  std::string target_frame_;
-  std::vector<std::string> source_frames_;
-  std::string base_frame_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
-  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-  rclcpp::TimerBase::SharedPtr timer_;
-};
+  if (!found) {
+    RCLCPP_DEBUG(this->get_logger(), "No dock found");
+    return;
+  }
 
-int main(int argc, char * argv[])
-{
-  rclcpp::init(argc, argv);
-  auto node = std::make_shared<DockPosePublisherNode>();
-  rclcpp::spin(node);
-  rclcpp::shutdown();
-  return 0;
+  pose_msg.pose.position.x = closest_dock.transform.translation.x;
+  pose_msg.pose.position.y = closest_dock.transform.translation.y;
+  pose_msg.pose.position.z = 0.0;
+  pose_msg.pose.orientation = closest_dock.transform.rotation;
+  pose_publisher_->publish(pose_msg);
 }
+}  // namespace panther_docking

panther_gazebo/README.md

@@ -14,6 +14,7 @@ The package contains a launch file and source files used to run the robot simula
 - [`battery_plugin_config.yaml`](./config/battery_plugin_config.yaml): Simulated LinearBatteryPlugin configuration.
 - [`gz_bridge.yaml`](./config/gz_bridge.yaml): Specify data to exchange between ROS and Gazebo simulation.
 - [`teleop_with_estop.config`](./config/teleop_with_estop.config): Gazebo layout configuration file, which adds E-Stop and Teleop widgets.
+- [`panther_docking_server.yaml`](../panther_docking/config/panther_docking_server.yaml): Defines poses for charging docks.
 
 ## ROS Nodes