From de79d2c5390d2ee3b6dc5a60ae6f78242ee5c347 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 22 Nov 2024 14:08:23 +0000 Subject: [PATCH] described parameters | applied codeguides Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 3 +- panther_docking/README.md | 3 + .../config/panther_docking_server.yaml | 4 +- .../dock_pose_publisher_node.hpp | 51 +++++ .../src/dock_pose_publisher_main.cpp | 40 ++++ .../src/dock_pose_publisher_node.cpp | 177 ++++++++---------- panther_gazebo/README.md | 1 + 7 files changed, 177 insertions(+), 102 deletions(-) create mode 100644 panther_docking/include/panther_docking/dock_pose_publisher_node.hpp create mode 100644 panther_docking/src/dock_pose_publisher_main.cpp diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index 7af73bfb..d0bb9d9a 100644 --- a/panther_docking/CMakeLists.txt +++ b/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}) diff --git a/panther_docking/README.md b/panther_docking/README.md index 54d16899..5ab93578 100644 --- a/panther_docking/README.md +++ b/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. +- `.apriltag_id` [*int*, default: **0**]: AprilTag ID of a dock. +- `.dock_frame` [*string*, default: **main_wibotic_receiver_requested_pose_link**]: A frame id to compare with fixed frame if docked. +- `.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. diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index c9de95c0..0b17f1b3 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -29,14 +29,14 @@ type: panther_charging_dock frame: /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: /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: diff --git a/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp b/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp new file mode 100644 index 00000000..875a9327 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include +#include + +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 source_frames_; + std::string base_frame_; + rclcpp::Publisher::SharedPtr pose_publisher_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; + rclcpp::TimerBase::SharedPtr timer_; +}; +} // namespace panther_docking + +#endif // PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ diff --git a/panther_docking/src/dock_pose_publisher_main.cpp b/panther_docking/src/dock_pose_publisher_main.cpp new file mode 100644 index 00000000..cd241dd5 --- /dev/null +++ b/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 +#include +#include + +#include + +#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("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; +} diff --git a/panther_docking/src/dock_pose_publisher_node.cpp b/panther_docking/src/dock_pose_publisher_node.cpp index bd59bbc2..7fd97319 100644 --- a/panther_docking/src/dock_pose_publisher_node.cpp +++ b/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 #include #include @@ -23,123 +25,100 @@ #include #include -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({"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(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({"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(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(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); - timer_ = this->create_wall_timer( - publish_period, std::bind(&DockPosePublisherNode::publishPose, this)); - pose_publisher_ = this->create_publisher( - "docking/dock_pose", 10); + timer_ = this->create_wall_timer( + publish_period, std::bind(&DockPosePublisherNode::publishPose, this)); + pose_publisher_ = this->create_publisher( + "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::max(); - double closest_dist = std::numeric_limits::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 source_frames_; - std::string base_frame_; - rclcpp::Publisher::SharedPtr pose_publisher_; - std::shared_ptr tf_listener_; - std::unique_ptr 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(); - 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 diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md index a4d3bb85..a724fb16 100644 --- a/panther_gazebo/README.md +++ b/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