diff --git a/panther_description/config/docking_components.yaml b/panther_description/config/docking_components.yaml index 940eb137..eb497cea 100644 --- a/panther_description/config/docking_components.yaml +++ b/panther_description/config/docking_components.yaml @@ -6,15 +6,15 @@ components: rpy: 0.0 0.0 0.0 device_namespace: wireless_charger - - type: WCH02 - parent_link: world - xyz: 3.0 -2.0 1.0 - rpy: 1.57 0.0 -1.57 - device_namespace: wireless_charger - - type: CAM01 name: camera parent_link: front_bumper_link xyz: 0.0 0.0 -0.06 rpy: 0.0 0.0 0.0 device_namespace: camera + + - type: LDR06 + parent_link: cover_link + xyz: 0.0 0.0 0.0 + rpy: 0.0 0.0 0.0 + device_namespace: main_lidar 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..47ab38ea 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -9,7 +9,8 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht ## Configuration Files -- [`panther_docking_server.yaml`](./config/panther_docking_server.yaml): Defines parameters for a `docking_server` and a `PantherChargingDock` plugin. +- [`panther_docking_server.yaml`](./config/panther_docking_server.yaml): Defines parameters for a `docking_server` and a `PantherChargingDock` plugin. Defines poses where charging docks are spawned in the Gazebo. + ## ROS Nodes @@ -53,3 +54,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/apriltag.yaml b/panther_docking/config/apriltag.yaml new file mode 100644 index 00000000..0e5de81e --- /dev/null +++ b/panther_docking/config/apriltag.yaml @@ -0,0 +1,21 @@ +--- +/**: + ros__parameters: + image_transport: raw + family: 36h11 + size: 0.06 + profile: false + + max_hamming: 0 + detector: + threads: 1 + decimate: 2.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + + tag: + ids: [0, 1] + frames: [main_apriltag_link, backup_apriltag_link] + sizes: [0.06, 0.06] diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index b0c3791e..2f7b223e 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -16,23 +16,32 @@ dock_plugins: ["panther_charging_dock"] panther_charging_dock: plugin: panther_docking::PantherChargingDock - external_detection_timeout: 0.1 + external_detection_timeout: 0.2 docking_distance_threshold: 0.12 - docking_yaw_threshold: 0.15 - staging_x_offset: -0.5 + docking_yaw_threshold: 0.2 + staging_x_offset: -1.3 filter_coef: 0.1 use_wibotic_info: wibotic_info_timeout: 1.0 - docks: ["main"] + docks: ["main", "backup"] main: type: panther_charging_dock - frame: /main_wibotic_receiver_requested_pose_link - pose: [0.0, 0.0, 0.0] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot + frame: /map + dock_frame: main_wibotic_receiver_requested_pose_link + 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] # [x, y, yaw] pose of the dock on the map. Used also for spawning dock in the simulation. + apriltag_id: 1 controller: k_phi: 1.0 k_delta: 2.0 v_linear_min: 0.0 - v_linear_max: 0.1 + v_linear_max: 0.3 v_angular_max: 0.15 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/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 8e0648b6..88d86105 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -149,8 +149,10 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * @brief Method to update and publish the staging pose. * * Uses staging_x_offset_ and staging_yaw_offset_ to calculate the staging pose. + * + * @param frame The frame to publish the staging pose in. */ - void updateAndPublishStagingPose(); + void updateAndPublishStagingPose(const std::string & frame); /** * @brief Dock pose callback, used for external detection. diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 68690e3f..f6c4b730 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -36,16 +36,18 @@ def generate_launch_description(): description=("Path to docking server configuration file."), ) - declare_use_docking_arg = DeclareLaunchArgument( - "use_docking", - default_value="True", - description="Enable docking server.", - choices=["True", "False", "true", "false"], + apriltag_config_path = LaunchConfiguration("apriltag_config_path") + declare_apriltag_config_path_arg = DeclareLaunchArgument( + "apriltag_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("panther_docking"), "config", "apriltag.yaml"] + ), + description=("Path to apriltag configuration file. Only used in simulation."), ) - namespace = LaunchConfiguration("namespace") - use_docking = LaunchConfiguration("use_docking") - use_sim = LaunchConfiguration("use_sim") + namespace = LaunchConfiguration("namespace", default="") + use_docking = LaunchConfiguration("use_docking", default="True") + use_sim = LaunchConfiguration("use_sim", default="False") log_level = LaunchConfiguration("log_level") declare_log_level = DeclareLaunchArgument( @@ -119,6 +121,20 @@ def generate_launch_description(): condition=IfCondition(use_docking), ) + apriltag_node = Node( + package="apriltag_ros", + executable="apriltag_node", + parameters=[{"use_sim_time": True}, apriltag_config_path], + namespace=namespace, + emulate_tty=True, + remappings={ + "camera_info": "camera/color/camera_info", + "image_rect": "camera/color/image_raw", + "detections": "docking/april_tags", + }.items(), + condition=IfCondition(PythonExpression([use_docking, " and ", use_sim])), + ) + station_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -143,27 +159,9 @@ def generate_launch_description(): ), ) - # FIXME: Remove before release - panther_manager_dir = FindPackageShare("panther_manager") - docking_manager_node = Node( - package="panther_manager", - executable="docking_manager_node", - name="docking_manager", - parameters=[ - PathJoinSubstitution([panther_manager_dir, "config", "docking_manager.yaml"]), - { - "bt_project_path": PathJoinSubstitution( - [panther_manager_dir, "behavior_trees", "DockingBT.btproj"] - ) - }, - ], - namespace=namespace, - emulate_tty=True, - ) - return LaunchDescription( [ - declare_use_docking_arg, + declare_apriltag_config_path_arg, declare_docking_server_config_path_arg, declare_log_level, declare_use_wibotic_info_arg, @@ -171,7 +169,7 @@ def generate_launch_description(): docking_server_node, docking_server_activate_node, dock_pose_publisher, + apriltag_node, wibotic_connector_can, - docking_manager_node, ] ) diff --git a/panther_docking/launch/station.launch.py b/panther_docking/launch/station.launch.py index c0f382ff..0dc66048 100644 --- a/panther_docking/launch/station.launch.py +++ b/panther_docking/launch/station.launch.py @@ -14,17 +14,19 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os +from tempfile import NamedTemporaryFile + import imageio +import yaml from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition from launch.substitutions import ( Command, - EnvironmentVariable, FindExecutable, LaunchConfiguration, PathJoinSubstitution, - PythonExpression, ) from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue @@ -36,19 +38,13 @@ def generate_apriltag_and_get_path(tag_id): tag_generator = TagGenerator2("tag36h11") tag_image = tag_generator.generate(tag_id, scale=1000) + temp_file = NamedTemporaryFile(suffix=f"_tag_{tag_id}.png", delete=False) - path = f"/tmp/tag_{tag_id}.png" - - imageio.imwrite(path, tag_image) - return path + imageio.imwrite(temp_file.name, tag_image) + return temp_file.name -def launch_setup(context, *args, **kwargs): - namespace = LaunchConfiguration("namespace").perform(context) - apriltag_id = int(LaunchConfiguration("apriltag_id").perform(context)) - apriltag_size = LaunchConfiguration("apriltag_size").perform(context) - use_docking = LaunchConfiguration("use_docking").perform(context) - +def generate_urdf(name, apriltag_id, apriltag_size): apriltag_image_path = generate_apriltag_and_get_path(apriltag_id) station_description_content = Command( @@ -63,51 +59,63 @@ def launch_setup(context, *args, **kwargs): ] ), " device_namespace:=", - "main", + name, " apriltag_image_path:=", apriltag_image_path, " apriltag_size:=", apriltag_size, ] ) + return station_description_content - namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"]) - - station_description = { - "robot_description": ParameterValue(station_description_content, value_type=str) - } - - station_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="wibotic_station_state_publisher", - parameters=[ - station_description, - {"frame_prefix": namespace_ext}, - ], - remappings=[("robot_description", "station_description")], - namespace=namespace, - emulate_tty=True, - condition=IfCondition(use_docking), - ) - return [station_state_pub_node] +def launch_stations_descriptions(context, *args, **kwargs): + apriltag_id = int(LaunchConfiguration("apriltag_id").perform(context)) + apriltag_size = LaunchConfiguration("apriltag_size").perform(context) + use_docking = LaunchConfiguration("use_docking").perform(context) + docking_server_config_path = LaunchConfiguration("docking_server_config_path").perform(context) + apriltag_size = LaunchConfiguration("apriltag_size").perform(context) -def generate_launch_description(): - declare_use_docking_arg = DeclareLaunchArgument( - "use_docking", - default_value="True", - description="Enable docking server.", - choices=["True", "False", "true", "false"], - ) + docking_server_config = None + + try: + with open(os.path.join(docking_server_config_path)) as file: + docking_server_config = yaml.safe_load(file) + if not isinstance(docking_server_config, dict) or "/**" not in docking_server_config: + raise ValueError("Invalid configuration structure") + except Exception as e: + print(f"Error loading docking server config: {str(e)}") + return [] + + actions = [] + ros_parameters = docking_server_config["/**"]["ros__parameters"] + docks_names = ros_parameters["docks"] + for dock_name in docks_names: + apriltag_id = ros_parameters[dock_name]["apriltag_id"] + station_description_content = generate_urdf(dock_name, apriltag_id, apriltag_size) + station_description = { + "robot_description": ParameterValue(station_description_content, value_type=str) + } + + station_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name=[dock_name, "_station_state_publisher"], + parameters=[ + station_description, + ], + remappings=[("robot_description", [dock_name, "_station_description"])], + emulate_tty=True, + condition=IfCondition(use_docking), + ) + + actions.append(station_state_pub_node) + + return actions - declare_namespace_arg = DeclareLaunchArgument( - "namespace", - default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), - description="Add namespace to all launched nodes.", - ) +def generate_launch_description(): declare_apriltag_id = DeclareLaunchArgument( "apriltag_id", default_value="0", @@ -120,12 +128,18 @@ def generate_launch_description(): description="Size in meters of a generated apriltag on the station", ) + declare_use_docking_arg = DeclareLaunchArgument( + "use_docking", + default_value="True", + description="Enable docking server and spawn docking stations in a simulation.", + choices=["True", "False", "true", "false"], + ) + return LaunchDescription( [ - declare_use_docking_arg, - declare_namespace_arg, declare_apriltag_id, declare_apriltag_size, - OpaqueFunction(function=launch_setup), + declare_use_docking_arg, + OpaqueFunction(function=launch_stations_descriptions), ] ) diff --git a/panther_docking/package.xml b/panther_docking/package.xml index 11351ff4..1c680705 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -28,9 +28,10 @@ tf2_ros wibotic_msgs + apriltag_ros nav2_lifecycle_manager python3-imageio - wibotic_connector_can + wibotic_connector_can xacro ament_cmake 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 2784ebf0..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,81 +25,100 @@ #include #include -class DockPosePublisherNode : public rclcpp::Node +namespace panther_docking +{ +DockPosePublisherNode::DockPosePublisherNode(const std::string & name) : Node(name) { -public: - DockPosePublisherNode() : Node("pose_publisher_node") - { - declare_parameter("publish_rate", 10.0); - declare_parameter("docks", std::vector({"main"})); - declare_parameter("fixed_frame", "odom"); - - const auto fixed_frame = get_parameter("fixed_frame").as_string(); - const auto docks = get_parameter("docks").as_string_array(); - - std::string dock_pose_frame_id; - for (const auto & dock : docks) { - declare_parameter(dock + ".type", "panther_charging_dock"); - declare_parameter(dock + ".frame", "main_wibotic_receiver_requested_pose_link"); - - const auto dock_type = get_parameter(dock + ".type").as_string(); - if (dock_type == "panther_charging_dock") { - dock_pose_frame_id = get_parameter(dock + ".frame").as_string(); - } + 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; - const auto publish_rate = get_parameter("publish_rate").as_double(); - const auto publish_period = std::chrono::duration(1.0 / publish_rate); + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); - source_frame_ = dock_pose_frame_id; - target_frame_ = fixed_frame; + timer_ = this->create_wall_timer( + publish_period, std::bind(&DockPosePublisherNode::publishPose, this)); + pose_publisher_ = this->create_publisher( + "docking/dock_pose", 10); - pose_publisher_ = this->create_publisher( - "docking/dock_pose", 10); + 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_; - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); + geometry_msgs::msg::TransformStamped closest_dock; + geometry_msgs::msg::TransformStamped base_transform_stamped; - timer_ = this->create_wall_timer( - publish_period, std::bind(&DockPosePublisherNode::publishPose, this)); + bool found = false; + + 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; } -private: - void publishPose() - { + 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_frame_, tf2::TimePointZero); + target_frame_, source_frames_[i], tf2::TimePointZero, tf2::durationFromSec(timeout_)); } catch (tf2::TransformException & ex) { - RCLCPP_DEBUG(this->get_logger(), "Could not get transform: %s", ex.what()); - return; + RCLCPP_DEBUG_STREAM(this->get_logger(), "Could not get transform: " << ex.what()); + continue; } - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.stamp = this->now(); - pose_msg.header.frame_id = target_frame_; - pose_msg.pose.position.x = transform_stamped.transform.translation.x; - pose_msg.pose.position.y = transform_stamped.transform.translation.y; - pose_msg.pose.position.z = 0.0; - pose_msg.pose.orientation = transform_stamped.transform.rotation; + 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); - pose_publisher_->publish(pose_msg); + if (dist < kMinimalDetectionDistance && dist < closest_dist) { + closest_dist = dist; + closest_dock = transform_stamped; + found = true; + } } - std::string target_frame_; - std::string source_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_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 9890e97e..744db722 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -130,15 +130,14 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( { RCLCPP_DEBUG_STREAM(logger_, "Getting staging pose in frame: " << frame); - // When there is no global pose to reach thanks to nav2 - if (pose == geometry_msgs::msg::Pose()) { - if (dock_pose_.header.frame_id.empty()) { - throw opennav_docking_core::FailedToDetectDock("No dock pose detected"); - } - - updateAndPublishStagingPose(); + // No global pose provided, use the detected dock pose + if (pose != geometry_msgs::msg::Pose()) { + dock_pose_.pose = pose; + dock_frame_ = frame; } + updateAndPublishStagingPose(frame); + return staging_pose_; } @@ -160,12 +159,15 @@ bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) auto duration = rclcpp::Time(request_detection_time) - rclcpp::Time(dock_pose_.header.stamp); if (duration > timeout) { RCLCPP_WARN_STREAM( - logger_, "Lost detection or did not detect: timeout exceeded: " << duration.seconds()); + logger_, "Detection timeout exceeded. Duration since last detection: " + << duration.seconds() << " seconds (timeout threshold: " << timeout.seconds() + << " seconds). " + << "No detection received or lost detection for external detection."); return false; } pose = dock_pose_; - updateAndPublishStagingPose(); + updateAndPublishStagingPose(fixed_frame_name_); return true; } @@ -229,12 +231,21 @@ void PantherChargingDock::setDockPose(const PoseStampedMsg::SharedPtr pose) dock_pose_ = filtered_pose; } -void PantherChargingDock::updateAndPublishStagingPose() +void PantherChargingDock::updateAndPublishStagingPose(const std::string & frame) { const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + RCLCPP_DEBUG_STREAM( + logger_, "Dock pose x: " << dock_pose_.pose.position.x << " y: " << dock_pose_.pose.position.y + << " yaw: " << yaw); + staging_pose_ = dock_pose_; - staging_pose_.pose.position.x += cos(yaw) * staging_x_offset_; - staging_pose_.pose.position.y += sin(yaw) * staging_x_offset_; + staging_pose_.header.frame_id = frame; + staging_pose_.header.stamp = node_.lock()->now(); + staging_pose_.pose.position.x += std::cos(yaw) * staging_x_offset_; + staging_pose_.pose.position.y += std::sin(yaw) * staging_x_offset_; + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, yaw); + staging_pose_.pose.orientation = tf2::toMsg(orientation); staging_pose_pub_->publish(staging_pose_); } diff --git a/panther_gazebo/launch/apriltag.launch.py b/panther_gazebo/launch/apriltag.launch.py deleted file mode 100644 index deeaa24e..00000000 --- a/panther_gazebo/launch/apriltag.launch.py +++ /dev/null @@ -1,62 +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. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from nav2_common.launch import ReplaceString - - -def generate_launch_description(): - use_docking = LaunchConfiguration("use_docking") - use_sim = LaunchConfiguration("use_sim") - namespace = LaunchConfiguration("namespace") - - apriltag_config_path = LaunchConfiguration("apriltag_config_path") - apriltag_config_path_arg = DeclareLaunchArgument( - "apriltag_config_path", - default_value=PathJoinSubstitution( - [FindPackageShare("panther_gazebo"), "config", "apriltag.yaml"] - ), - description=("Path to apriltag configuration file."), - ) - - namespaced_apriltag_config_path = ReplaceString( - source_file=apriltag_config_path, - replacements={"": namespace, "//": "/"}, - ) - - apriltag_node = Node( - package="apriltag_ros", - executable="apriltag_node", - parameters=[{"use_sim_time": use_sim}, namespaced_apriltag_config_path], - namespace=namespace, - emulate_tty=True, - remappings={ - "camera_info": "camera/color/camera_info", - "image_rect": "camera/color/image_raw", - "detections": "docking/april_tags", - }.items(), - condition=IfCondition(use_docking), - ) - - return LaunchDescription( - [ - apriltag_config_path_arg, - apriltag_node, - ] - ) diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index 8f7fa280..aa55478a 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -198,19 +198,6 @@ def generate_launch_description(): launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), ) - apriltag_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("panther_gazebo"), - "launch", - "apriltag.launch.py", - ] - ), - ), - launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), - ) - return LaunchDescription( [ declare_battery_config_path_arg, @@ -227,6 +214,5 @@ def generate_launch_description(): simulate_components, gz_bridge, docking_launch, - apriltag_launch, ] ) diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py index 04c192d7..533e2440 100644 --- a/panther_gazebo/launch/simulation.launch.py +++ b/panther_gazebo/launch/simulation.launch.py @@ -71,6 +71,14 @@ def generate_launch_description(): ), ) + spawn_charging_docks = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("panther_gazebo"), "launch", "spawn_charging_docks.launch.py"] + ) + ), + ) + return LaunchDescription( [ declare_gz_gui, @@ -79,5 +87,6 @@ def generate_launch_description(): SetUseSimTime(True), gz_sim, simulate_robots, + spawn_charging_docks, ] ) diff --git a/panther_gazebo/launch/spawn_charging_docks.launch.py b/panther_gazebo/launch/spawn_charging_docks.launch.py new file mode 100644 index 00000000..2f626618 --- /dev/null +++ b/panther_gazebo/launch/spawn_charging_docks.launch.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Husarion sp. z o.o. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from tempfile import NamedTemporaryFile + +import imageio +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_apriltag_and_get_path(tag_id): + from moms_apriltag import TagGenerator2 + + tag_generator = TagGenerator2("tag36h11") + tag_image = tag_generator.generate(tag_id, scale=1000) + temp_file = NamedTemporaryFile(suffix=f"_tag_{tag_id}.png", delete=False) + + imageio.imwrite(temp_file.name, tag_image) + return temp_file.name + + +def spawn_stations(context, *args, **kwargs): + docking_server_config_path = LaunchConfiguration("docking_server_config_path").perform(context) + use_docking = LaunchConfiguration("use_docking").perform(context) + docking_server_config = None + + try: + with open(os.path.join(docking_server_config_path)) as file: + docking_server_config = yaml.safe_load(file) + if not isinstance(docking_server_config, dict) or "/**" not in docking_server_config: + raise ValueError("Invalid configuration structure") + except Exception as e: + print(f"Error loading docking server config: {str(e)}") + return [] + + actions = [] + + ros_parameters = docking_server_config["/**"]["ros__parameters"] + docks_names = ros_parameters["docks"] + for dock_name in docks_names: + pose = ros_parameters[dock_name]["pose"] + + spawn_station = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + [dock_name, "_station"], + "-topic", + [dock_name, "_station_description"], + "-x", + str(pose[0]), + "-y", + str(pose[1] - 2.0), # -2.0 is the offset between world and map + "-z", + "0.2", # station z is not in 0.0 + "-R", + "1.57", + "-P", + "0.0", + "-Y", + str(pose[2] - 1.57), + ], + emulate_tty=True, + condition=IfCondition(use_docking), + ) + + actions.append(spawn_station) + + return actions + + +def generate_launch_description(): + declare_device_namespace = DeclareLaunchArgument( + "device_namespace", + default_value="", + description="Device namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.", + ) + + declare_docking_server_config_path_arg = DeclareLaunchArgument( + "docking_server_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("panther_docking"), "config", "panther_docking_server.yaml"] + ), + description=("Path to docking server configuration file."), + ) + + declare_use_docking_arg = DeclareLaunchArgument( + "use_docking", + default_value="True", + description="Enable docking server and spawn docking stations in a simulation.", + choices=["True", "False", "true", "false"], + ) + + return LaunchDescription( + [ + declare_docking_server_config_path_arg, + declare_device_namespace, + declare_use_docking_arg, + OpaqueFunction(function=spawn_stations), + ] + ) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index ecadf84c..184e07c1 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -27,7 +27,6 @@ std_msgs std_srvs - apriltag_ros controller_manager husarion_gz_worlds launch diff --git a/panther_manager/launch/manager.launch.py b/panther_manager/launch/manager.launch.py index c2ab3b18..36703244 100644 --- a/panther_manager/launch/manager.launch.py +++ b/panther_manager/launch/manager.launch.py @@ -87,6 +87,7 @@ def generate_launch_description(): description="Whether simulation is used", ) + use_docking = LaunchConfiguration("use_docking", default="False") docking_manager_node = Node( package="panther_manager", executable="docking_manager_node", @@ -97,6 +98,7 @@ def generate_launch_description(): ], namespace=namespace, emulate_tty=True, + condition=IfCondition(use_docking), ) lights_manager_node = Node(