diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 2027f4c0e1c..69fe42195eb 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -421,6 +421,13 @@ docking_server: k_delta: 2.0 v_linear_min: 0.15 v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "/local_costmap/costmap_raw" + footprint_topic: "/local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 loopback_simulator: ros__parameters: diff --git a/nav2_docking/README.md b/nav2_docking/README.md index fba8ca11394..2a9ba502ae9 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -208,14 +208,21 @@ For debugging purposes, there are several publishers which can be used with RVIZ | dock_database | The filepath to the dock database to use for this environment | string | N/A | | docks | Instead of `dock_database`, the set of docks specified in the params file itself | vector | N/A | | navigator_bt_xml | BT XML to use for Navigator, if non-default | string | "" | -| controller.k_phi | TODO | double | 3.0 | -| controller.k_delta | TODO | double | 2.0 | -| controller.beta | TODO | double | 0.4 | -| controller.lambda | TODO | double | 2.0 | -| controller.v_linear_min | TODO | double | 0.1 | -| controller.v_linear_max | TODO | double | 0.25 | -| controller.v_angular_max | TODO | double | 0.75 | -| controller.slowdown_radius | TODO | double | 0.25 | +| controller.k_phi | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem | double | 3.0 | +| controller.k_delta | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem | double | 2.0 | +| controller.beta | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases | double | 0.4 | +| controller.lambda | Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves | double | 2.0 | +| controller.v_linear_min | Minimum linear velocity (m/s) | double | 0.1 | +| controller.v_linear_max | Maximum linear velocity (m/s) | double | 0.25 | +| controller.v_angular_max | Maximum angular velocity (rad/s) produced by the control law | double | 0.75 | +| controller.slowdown_radius | Radius (m) around the goal pose in which the robot will start to slow down | double | 0.25 | +| controller.use_collision_detection | Whether to use collision detection to avoid obstacles | bool | true | +| controller.costmap_topic | The topic to use for the costmap | string | "local_costmap/costmap_raw" | +| controller.footprint_topic | The topic to use for the robot's footprint | string | "local_costmap/published_footprint" | +| controller.transform_tolerance | Time with which to post-date the transform that is published, to indicate that this transform is valid into the future. | double | 0.1 | +| controller.projection_time | Time to look ahead for collisions (s). | double | 5.0 | +| controller.simulation_time_step | Time step for projections (s). | double | 0.1 | +| controller.dock_collision_threshold | Distance (m) from the dock pose to ignore collisions. | double | 0.3 | Note: `dock_plugins` and either `docks` or `dock_database` are required. diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index afae5965d22..142b4beaf7d 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,11 +17,16 @@ #define OPENNAV_DOCKING__CONTROLLER_HPP_ #include +#include #include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/footprint_subscriber.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" #include "nav2_graceful_controller/smooth_control_law.hpp" +#include "nav_msgs/msg/path.hpp" #include "nav2_util/lifecycle_node.hpp" namespace opennav_docking @@ -34,36 +40,91 @@ class Controller public: /** * @brief Create a controller instance. Configure ROS 2 parameters. + * + * @param node Lifecycle node + * @param tf tf2_ros TF buffer + * @param fixed_frame Fixed frame + * @param base_frame Robot base frame */ - explicit Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); + Controller( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr tf, + std::string fixed_frame, std::string base_frame); + + /** + * @brief A destructor for opennav_docking::Controller + */ + ~Controller(); /** * @brief Compute a velocity command using control law. * @param pose Target pose, in robot centric coordinates. * @param cmd Command velocity. + * @param is_docking If true, robot is docking. If false, robot is undocking. * @param backward If true, robot will drive backwards to goal. * @returns True if command is valid, false otherwise. */ bool computeVelocityCommand( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking, bool backward = false); +protected: + /** + * @brief Check if a trajectory is collision free. + * + * @param target_pose Target pose, in robot centric coordinates. + * @param is_docking If true, robot is docking. If false, robot is undocking. + * @param backward If true, robot will drive backwards to goal. + * @return True if trajectory is collision free. + */ + bool isTrajectoryCollisionFree( + const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward = false); + /** - * @brief Callback executed when a parameter change is detected + * @brief Callback executed when a parameter change is detected. + * * @param event ParameterEvent message */ rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + /** + * @brief Configure the collision checker. + * + * @param node Lifecycle node + * @param costmap_topic Costmap topic + * @param footprint_topic Footprint topic + * @param transform_tolerance Transform tolerance + */ + void configureCollisionChecker( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + std::string costmap_topic, std::string footprint_topic, double transform_tolerance); + // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; -protected: - std::unique_ptr control_law_; + rclcpp::Logger logger_{rclcpp::get_logger("Controller")}; + rclcpp::Clock::SharedPtr clock_; + // Smooth control law + std::unique_ptr control_law_; double k_phi_, k_delta_, beta_, lambda_; double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_; + + // The trajectory of the robot while dock / undock for visualization / debug purposes + rclcpp::Publisher::SharedPtr trajectory_pub_; + + // Used for collision checking + bool use_collision_detection_; + double projection_time_; + double simulation_time_step_; + double dock_collision_threshold_; + double transform_tolerance_; + std::shared_ptr tf2_buffer_; + std::unique_ptr costmap_sub_; + std::unique_ptr footprint_sub_; + std::shared_ptr collision_checker_; + std::string fixed_frame_, base_frame_; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index 08c3cc1f7c2..9e03c5b154c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -112,12 +112,13 @@ class DockingServer : public nav2_util::LifecycleNode * @param pose The pose to command towards. * @param linear_tolerance Pose is reached when linear distance is within this tolerance. * @param angular_tolerance Pose is reached when angular distance is within this tolerance. + * @param is_docking If true, the robot is docking. If false, the robot is undocking. * @param backward If true, the robot will drive backwards. * @returns True if pose is reached. */ bool getCommandToPose( geometry_msgs::msg::Twist & cmd, const geometry_msgs::msg::PoseStamped & pose, - double linear_tolerance, double angular_tolerance, bool backward); + double linear_tolerance, double angular_tolerance, bool is_docking, bool backward); /** * @brief Get the robot pose (aka base_frame pose) in another frame. diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 8613f2d27fc..c0e45ee4223 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,13 +17,22 @@ #include "rclcpp/rclcpp.hpp" #include "opennav_docking/controller.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" +#include "nav_2d_utils/conversions.hpp" +#include "tf2/utils.h" namespace opennav_docking { -Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) +Controller::Controller( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr tf, + std::string fixed_frame, std::string base_frame) +: tf2_buffer_(tf), fixed_frame_(fixed_frame), base_frame_(base_frame) { + logger_ = node->get_logger(); + clock_ = node->get_clock(); + nav2_util::declare_parameter_if_not_declared( node, "controller.k_phi", rclcpp::ParameterValue(3.0)); nav2_util::declare_parameter_if_not_declared( @@ -39,6 +49,22 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) node, "controller.v_angular_max", rclcpp::ParameterValue(0.75)); nav2_util::declare_parameter_if_not_declared( node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.use_collision_detection", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", + rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue( + std::string("local_costmap/published_footprint"))); + nav2_util::declare_parameter_if_not_declared( + node, "controller.transform_tolerance", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(5.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); node->get_parameter("controller.k_phi", k_phi_); node->get_parameter("controller.k_delta", k_delta_); @@ -55,16 +81,120 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1)); + + node->get_parameter("controller.use_collision_detection", use_collision_detection_); + node->get_parameter("controller.projection_time", projection_time_); + node->get_parameter("controller.simulation_time_step", simulation_time_step_); + node->get_parameter("controller.transform_tolerance", transform_tolerance_); + + if (use_collision_detection_) { + std::string costmap_topic, footprint_topic; + node->get_parameter("controller.costmap_topic", costmap_topic); + node->get_parameter("controller.footprint_topic", footprint_topic); + node->get_parameter("controller.dock_collision_threshold", dock_collision_threshold_); + configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_); + } + + trajectory_pub_ = + node->create_publisher("docking_trajectory", 1); +} + +Controller::~Controller() +{ + control_law_.reset(); + trajectory_pub_.reset(); + collision_checker_.reset(); + costmap_sub_.reset(); + footprint_sub_.reset(); } bool Controller::computeVelocityCommand( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward) + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking, + bool backward) { std::lock_guard lock(dynamic_params_lock_); cmd = control_law_->calculateRegularVelocity(pose, backward); + return isTrajectoryCollisionFree(pose, is_docking, backward); +} + +bool Controller::isTrajectoryCollisionFree( + const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward) +{ + // Visualization of the trajectory + nav_msgs::msg::Path trajectory; + trajectory.header.frame_id = base_frame_; + trajectory.header.stamp = clock_->now(); + + // First pose + geometry_msgs::msg::PoseStamped next_pose; + next_pose.header.frame_id = base_frame_; + trajectory.poses.push_back(next_pose); + + // Get the transform from base_frame to fixed_frame + geometry_msgs::msg::TransformStamped base_to_fixed_transform; + try { + base_to_fixed_transform = tf2_buffer_->lookupTransform( + fixed_frame_, base_frame_, trajectory.header.stamp, + tf2::durationFromSec(transform_tolerance_)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, "Could not get transform from %s to %s: %s", + base_frame_.c_str(), fixed_frame_.c_str(), ex.what()); + return false; + } + + // Generate path + for (double t = 0; t < projection_time_; t += simulation_time_step_) { + // Apply velocities to calculate next pose + next_pose.pose = control_law_->calculateNextPose( + simulation_time_step_, target_pose, next_pose.pose, backward); + + // Add the pose to the trajectory for visualization + trajectory.poses.push_back(next_pose); + + // Transform pose from base_frame into fixed_frame + geometry_msgs::msg::PoseStamped local_pose = next_pose; + local_pose.header.stamp = trajectory.header.stamp; + tf2::doTransform(local_pose, local_pose, base_to_fixed_transform); + + // Determine the distance at which to check for collisions + // Skip the final segment of the trajectory for docking + // and the initial segment for undocking + // This avoids false positives when the robot is at the dock + double dock_collision_distance = is_docking ? + nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose) : + std::hypot(next_pose.pose.position.x, next_pose.pose.position.y); + + // If this distance is greater than the dock_collision_threshold, check for collisions + if (use_collision_detection_ && + dock_collision_distance > dock_collision_threshold_ && + !collision_checker_->isCollisionFree(nav_2d_utils::poseToPose2D(local_pose.pose))) + { + RCLCPP_WARN( + logger_, "Collision detected at pose: (%.2f, %.2f, %.2f) in frame %s", + local_pose.pose.position.x, local_pose.pose.position.y, local_pose.pose.position.z, + local_pose.header.frame_id.c_str()); + trajectory_pub_->publish(trajectory); + return false; + } + } + + trajectory_pub_->publish(trajectory); + return true; } +void Controller::configureCollisionChecker( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + std::string costmap_topic, std::string footprint_topic, double transform_tolerance) +{ + costmap_sub_ = std::make_unique(node, costmap_topic); + footprint_sub_ = std::make_unique( + node, footprint_topic, *tf2_buffer_, base_frame_, transform_tolerance); + collision_checker_ = std::make_shared( + *costmap_sub_, *footprint_sub_, node->get_name()); +} + rcl_interfaces::msg::SetParametersResult Controller::dynamicParametersCallback(std::vector parameters) { @@ -92,6 +222,12 @@ Controller::dynamicParametersCallback(std::vector parameters) v_angular_max_ = parameter.as_double(); } else if (name == "controller.slowdown_radius") { slowdown_radius_ = parameter.as_double(); + } else if (name == "controller.projection_time") { + projection_time_ = parameter.as_double(); + } else if (name == "controller.simulation_time_step") { + simulation_time_step_ = parameter.as_double(); + } else if (name == "controller.dock_collision_threshold") { + dock_collision_threshold_ = parameter.as_double(); } // Update the smooth control law with the new params diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index b350cb40262..d7af36c9ebc 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -86,7 +86,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) // Create composed utilities mutex_ = std::make_shared(); - controller_ = std::make_unique(node); + controller_ = std::make_unique(node, tf2_buffer_, fixed_frame_, base_frame_); navigator_ = std::make_unique(node); dock_db_ = std::make_unique(mutex_); if (!dock_db_->initialize(node, tf2_buffer_)) { @@ -248,8 +248,7 @@ void DockingServer::dockRobot() // Send robot to its staging pose publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE); const auto initial_staging_pose = dock->getStagingPose(); - const auto robot_pose = getRobotPoseInFrame( - initial_staging_pose.header.frame_id); + const auto robot_pose = getRobotPoseInFrame(initial_staging_pose.header.frame_id); if (!goal->navigate_to_staging_pose || utils::l2Norm(robot_pose.pose, initial_staging_pose.pose) < dock_prestaging_tolerance_) { @@ -450,7 +449,9 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & // Compute and publish controls auto command = std::make_unique(); command->header.stamp = now(); - if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, dock_backwards_)) { + if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, true, + dock_backwards_)) + { throw opennav_docking_core::FailedToControl("Failed to get control"); } vel_publisher_->publish(std::move(command)); @@ -516,7 +517,7 @@ bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & stagin auto command = std::make_unique(); command->header.stamp = now(); if (getCommandToPose( - command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, + command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false, !dock_backwards_)) { return true; @@ -534,7 +535,7 @@ bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & stagin bool DockingServer::getCommandToPose( geometry_msgs::msg::Twist & cmd, const geometry_msgs::msg::PoseStamped & pose, - double linear_tolerance, double angular_tolerance, bool backward) + double linear_tolerance, double angular_tolerance, bool is_docking, bool backward) { // Reset command to zero velocity cmd.linear.x = 0; @@ -557,7 +558,7 @@ bool DockingServer::getCommandToPose( tf2_buffer_->transform(target_pose, target_pose, base_frame_); // Compute velocity command - if (!controller_->computeVelocityCommand(target_pose.pose, cmd, backward)) { + if (!controller_->computeVelocityCommand(target_pose.pose, cmd, is_docking, backward)) { throw opennav_docking_core::FailedToControl("Failed to get control"); } @@ -644,7 +645,7 @@ void DockingServer::undockRobot() auto command = std::make_unique(); command->header.stamp = now(); if (getCommandToPose( - command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, + command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false, !dock_backwards_)) { RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index c2f1e25f1c1..e0156cf727f 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,6 +18,10 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/pose.hpp" #include "opennav_docking/controller.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2_ros/buffer.h" #include "ament_index_cpp/get_package_share_directory.hpp" // Testing the controller at high level; the nav2_graceful_controller @@ -33,21 +38,187 @@ RosLockGuard g_rclcpp; namespace opennav_docking { +class ControllerFixture : public opennav_docking::Controller +{ +public: + ControllerFixture( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr tf, + std::string fixed_frame, std::string base_frame) + : Controller(node, tf, fixed_frame, base_frame) + { + } + + ~ControllerFixture() = default; + + bool isTrajectoryCollisionFree( + const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward = false) + { + return opennav_docking::Controller::isTrajectoryCollisionFree( + target_pose, is_docking, backward); + } + + void setCollisionTolerance(double tolerance) + { + dock_collision_threshold_ = tolerance; + } +}; + +class TestCollisionChecker : public nav2_util::LifecycleNode +{ +public: + explicit TestCollisionChecker(std::string name) + : LifecycleNode(name) + { + } + + ~TestCollisionChecker() + { + footprint_pub_.reset(); + costmap_pub_.reset(); + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) + { + RCLCPP_INFO(this->get_logger(), "Configuring"); + + costmap_ = std::make_shared(100, 100, 0.1, -5.0, -5.0); + + footprint_pub_ = create_publisher( + "test_footprint", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + costmap_pub_ = std::make_shared( + shared_from_this(), costmap_.get(), "test_base_frame", "test_costmap", true); + + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) + { + RCLCPP_INFO(this->get_logger(), "Activating"); + costmap_pub_->on_activate(); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) + { + RCLCPP_INFO(this->get_logger(), "Deactivating"); + costmap_pub_->on_deactivate(); + costmap_.reset(); + return nav2_util::CallbackReturn::SUCCESS; + } + + void publishFootprint( + const double radius, const double center_x, const double center_y, + std::string base_frame, const rclcpp::Time & stamp) + { + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = base_frame; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 p; + + p.x = center_x + radius; + p.y = center_y + radius; + msg->polygon.points.push_back(p); + + p.x = center_x + radius; + p.y = center_y - radius; + msg->polygon.points.push_back(p); + + p.x = center_x - radius; + p.y = center_y - radius; + msg->polygon.points.push_back(p); + + p.x = center_x - radius; + p.y = center_y + radius; + msg->polygon.points.push_back(p); + + footprint_pub_->publish(std::move(msg)); + } + + void publishCostmap() + { + costmap_pub_->publishCostmap(); + } + + geometry_msgs::msg::Pose setPose(double x, double y, double theta) + { + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = 0.0; + pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta); + return pose; + } + + void setRectangle( + double width, double height, double center_x, double center_y, unsigned char cost) + { + unsigned int mx, my; + if (!costmap_->worldToMap(center_x, center_y, mx, my)) { + RCLCPP_ERROR(this->get_logger(), "Failed to convert world coordinates to map coordinates"); + return; + } + + unsigned int width_cell = static_cast(width / costmap_->getResolution()); + unsigned int height_cell = static_cast(height / costmap_->getResolution()); + + for (unsigned int i = 0; i < width_cell; ++i) { + for (unsigned int j = 0; j < height_cell; ++j) { + costmap_->setCost(mx + i, my + j, cost); + } + } + } + + void clearCostmap() + { + if (!costmap_) { + RCLCPP_ERROR(this->get_logger(), "Costmap is not initialized"); + return; + } + + unsigned int size_x = costmap_->getSizeInCellsX(); + unsigned int size_y = costmap_->getSizeInCellsY(); + + for (unsigned int i = 0; i < size_x; ++i) { + for (unsigned int j = 0; j < size_y; ++j) { + costmap_->setCost(i, j, nav2_costmap_2d::FREE_SPACE); + } + } + } + +private: + std::shared_ptr costmap_; + + rclcpp::Publisher::SharedPtr footprint_pub_; + std::shared_ptr costmap_pub_; +}; + TEST(ControllerTests, ObjectLifecycle) { auto node = std::make_shared("test"); - auto controller = std::make_unique(node); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + // Skip collision detection + nav2_util::declare_parameter_if_not_declared( + node, "controller.use_collision_detection", rclcpp::ParameterValue(false)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); geometry_msgs::msg::Pose pose; geometry_msgs::msg::Twist cmd_out, cmd_init; - EXPECT_TRUE(controller->computeVelocityCommand(pose, cmd_out)); + EXPECT_TRUE(controller->computeVelocityCommand(pose, cmd_out, true)); EXPECT_NE(cmd_init, cmd_out); controller.reset(); } TEST(ControllerTests, DynamicParameters) { auto node = std::make_shared("test"); - auto controller = std::make_shared(node); + auto controller = std::make_unique( + node, nullptr, "test_base_frame", "test_base_frame"); auto params = std::make_shared( node->get_node_base_interface(), node->get_node_topics_interface(), @@ -63,7 +234,10 @@ TEST(ControllerTests, DynamicParameters) { rclcpp::Parameter("controller.v_linear_min", 5.0), rclcpp::Parameter("controller.v_linear_max", 6.0), rclcpp::Parameter("controller.v_angular_max", 7.0), - rclcpp::Parameter("controller.slowdown_radius", 8.0)}); + rclcpp::Parameter("controller.slowdown_radius", 8.0), + rclcpp::Parameter("controller.projection_time", 9.0), + rclcpp::Parameter("controller.simulation_time_step", 10.0), + rclcpp::Parameter("controller.dock_collision_threshold", 11.0)}); // Spin rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); @@ -77,6 +251,299 @@ TEST(ControllerTests, DynamicParameters) { EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0); EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0); EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0); + EXPECT_EQ(node->get_parameter("controller.projection_time").as_double(), 9.0); + EXPECT_EQ(node->get_parameter("controller.simulation_time_step").as_double(), 10.0); + EXPECT_EQ(node->get_parameter("controller.dock_collision_threshold").as_double(), 11.0); } +TEST(ControllerTests, TFException) +{ + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + auto controller = std::make_unique( + node, tf, "test_fixed_frame", "test_base_frame"); + + geometry_msgs::msg::Pose pose; + EXPECT_FALSE(controller->isTrajectoryCollisionFree(pose, false)); + controller.reset(); +} + +TEST(ControllerTests, CollisionCheckerDockForward) { + auto collision_tester = std::make_shared("collision_test"); + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); + collision_tester->configure(); + collision_tester->activate(); + + // Set the pose of the dock at 1.75m in front of the robot + auto dock_pose = collision_tester->setPose(1.75, 0.0, 0.0); + + // Publish a footprint of 0.5m "radius" at origin + auto radius = 0.5; + collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now()); + + // Publish an empty costmap + // It should not hit anything in an empty costmap + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); + + // Set a dock in the costmap of 0.2x1.5m at 2m in front of the robot + // It should hit the dock because the robot is 0.5m wide and the dock pose is at 1.75 + // But it does not hit because the collision tolerance is 0.3m + collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); + + // Set an object between the robot and the dock + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, 1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); + + // Set the collision tolerance to 0 to ensure all obstacles in the path are detected + controller->setCollisionTolerance(0.0); + + // Set a dock in the costmap of 0.2x1.5m at 2m in front of the robot + // Now it should hit the dock + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); + + collision_tester->deactivate(); +} + +TEST(ControllerTests, CollisionCheckerDockBackward) { + auto collision_tester = std::make_shared("collision_test"); + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); + collision_tester->configure(); + collision_tester->activate(); + + // Set the pose of the dock at 1.75m behind the robot + auto dock_pose = collision_tester->setPose(-1.75, 0.0, 0.0); + + // Publish a footprint of 0.5m "radius" at origin + auto radius = 0.5; + collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now()); + + // Publish an empty costmap + // It should not hit anything in an empty costmap + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); + + // Set a dock in the costmap of 0.2x1.5m at 2m behind the robot + // It should hit the dock because the robot is 0.5m wide and the dock pose is at -1.75 + // But it does not hit because the collision tolerance is 0.3m + collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); + + // Set an object between the robot and the dock + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, -1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); + + // Set the collision tolerance to 0 to ensure all obstacles in the path are detected + controller->setCollisionTolerance(0.0); + + // Set a dock in the costmap of 0.2x1.5m at 2m behind the robot + // Now it should hit the dock + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); + + collision_tester->deactivate(); +} + +TEST(ControllerTests, CollisionCheckerUndockBackward) { + auto collision_tester = std::make_shared("collision_test"); + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); + collision_tester->configure(); + collision_tester->activate(); + + // Set the staging pose at 1.75m behind the robot + auto staging_pose = collision_tester->setPose(-1.75, 0.0, 0.0); + + // Publish a footprint of 0.5m "radius" at origin + auto radius = 0.5; + collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now()); + + // Publish an empty costmap + // It should not hit anything in an empty costmap + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + // Set a dock in the costmap of 0.2x1.5m in front of the robot. The robot is docked + // It should hit the dock because the robot is 0.5m wide and the robot pose is at 1.75 + // But it does not hit because the collision tolerance is 0.3m + collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + // Set an object beyond the staging pose + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, -1.75 - 0.5, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + // Set an object between the robot and the staging pose + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, -1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + // Set the collision tolerance to 0 to ensure all obstacles in the path are detected + controller->setCollisionTolerance(0.0); + + // Set a dock in the costmap of 0.2x1.5m in front of the robot. The robot is docked + // Now it should hit the dock + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + collision_tester->deactivate(); +} + +TEST(ControllerTests, CollisionCheckerUndockForward) { + auto collision_tester = std::make_shared("collision_test"); + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); + collision_tester->configure(); + collision_tester->activate(); + + // Set the staging pose at 1.75m in the front of the robot + auto staging_pose = collision_tester->setPose(1.75, 0.0, 0.0); + + // Publish a footprint of 0.5m "radius" + auto radius = 0.5; + collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now()); + + // Publish an empty costmap + // It should not hit anything in an empty costmap + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + // Set a dock in the costmap of 0.2x1.5m at 0.5m behind the robot. The robot is docked + // It should not hit anything because the robot is docked and the trajectory is backward + collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + // Set an object beyond the staging pose + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.3, 1.75 + 0.5, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + // Set an object between the robot and the staging pose + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, 1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + // Set the collision tolerance to 0 to ensure all obstacles in the path are detected + controller->setCollisionTolerance(0.0); + + // Set a dock in the costmap of 0.2x1.5m at 0.5m behind the robot. The robot is docked + // Now it should hit the dock + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + collision_tester->deactivate(); +} + + } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 563e810ce36..5b76f6095ce 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -41,15 +41,19 @@ def generate_test_description(): executable='opennav_docking', name='docking_server', parameters=[{'wait_charge_timeout': 1.0, + 'controller': { + 'use_collision_detection': False, + 'transform_tolerance': 0.5, + }, 'dock_plugins': ['test_dock_plugin'], 'test_dock_plugin': { - 'plugin': 'opennav_docking::SimpleChargingDock', - 'use_battery_status': True}, + 'plugin': 'opennav_docking::SimpleChargingDock', + 'use_battery_status': True}, 'docks': ['test_dock'], 'test_dock': { - 'type': 'test_dock_plugin', - 'frame': 'odom', - 'pose': [10.0, 0.0, 0.0] + 'type': 'test_dock_plugin', + 'frame': 'odom', + 'pose': [10.0, 0.0, 0.0] }}], output='screen', ),