diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 8e345abe0d7..924d1ebc28a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -53,11 +53,11 @@ class BtActionServer const std::string & action_name, const std::vector & plugin_lib_names, const std::string & default_bt_xml_filename, - const std::vector & search_directories, OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback, - OnCompletionCallback on_completion_callback); + OnCompletionCallback on_completion_callback, + const std::vector & search_directories = std::vector{}); /** * @brief A destructor for nav2_behavior_tree::BtActionServer class diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index b8fb983f91c..10a8289d5f4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -39,11 +39,11 @@ BtActionServer::BtActionServer( const std::string & action_name, const std::vector & plugin_lib_names, const std::string & default_bt_xml_filename, - const std::vector & search_directories, OnGoalReceivedCallback on_goal_received_callback, OnLoopCallback on_loop_callback, OnPreemptCallback on_preempt_callback, - OnCompletionCallback on_completion_callback) + OnCompletionCallback on_completion_callback, + const std::vector & search_directories) : action_name_(action_name), default_bt_xml_filename_(default_bt_xml_filename), search_directories_(search_directories), diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index 3b2a3b382c1..96e9a594447 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -202,8 +202,10 @@ class BTActionNodeTestFixture : public ::testing::Test action_server_ = std::make_shared(); server_thread_ = std::make_shared( []() { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(action_server_); while (rclcpp::ok() && BTActionNodeTestFixture::action_server_ != nullptr) { - rclcpp::spin_some(BTActionNodeTestFixture::action_server_); + executor.spin_some(); std::this_thread::sleep_for(100ns); } }); @@ -211,6 +213,8 @@ class BTActionNodeTestFixture : public ::testing::Test void TearDown() override { + // Sleep for some time to avoid race condition + std::this_thread::sleep_for(std::chrono::milliseconds(80)); action_server_.reset(); tree_.reset(); server_thread_->join(); diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 9f686ad2ed4..8de6cb2b77b 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -31,6 +31,8 @@ class ControllerSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("controller_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -67,6 +69,7 @@ class ControllerSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -76,12 +79,15 @@ class ControllerSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr ControllerSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr ControllerSelectorTestFixture::executor_ = + nullptr; BT::NodeConfiguration * ControllerSelectorTestFixture::config_ = nullptr; std::shared_ptr ControllerSelectorTestFixture::factory_ = nullptr; @@ -126,7 +132,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); controller_selector_pub->publish(selected_controller_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check controller updated @@ -173,7 +179,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); controller_selector_pub->publish(selected_controller_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check controller updated diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index 0067a5bd7ed..c0d0ee35daa 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -31,6 +31,8 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("goal_checker_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -67,6 +69,7 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -76,12 +79,15 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr GoalCheckerSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr GoalCheckerSelectorTestFixture::executor_ = + nullptr; BT::NodeConfiguration * GoalCheckerSelectorTestFixture::config_ = nullptr; std::shared_ptr GoalCheckerSelectorTestFixture::factory_ = nullptr; @@ -127,7 +133,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); goal_checker_selector_pub->publish(selected_goal_checker_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check goal_checker updated @@ -175,7 +181,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); goal_checker_selector_pub->publish(selected_goal_checker_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check goal_checker updated diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index 7043d193789..82ea0635f27 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -31,6 +31,8 @@ class PlannerSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("planner_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -65,6 +67,7 @@ class PlannerSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -74,12 +77,15 @@ class PlannerSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr PlannerSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr PlannerSelectorTestFixture::executor_ = + nullptr; BT::NodeConfiguration * PlannerSelectorTestFixture::config_ = nullptr; std::shared_ptr PlannerSelectorTestFixture::factory_ = nullptr; @@ -125,7 +131,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); planner_selector_pub->publish(selected_planner_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check planner updated @@ -173,7 +179,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); planner_selector_pub->publish(selected_planner_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check planner updated diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp index eebb6c193b4..c305471f412 100644 --- a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp @@ -30,6 +30,8 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("progress_checker_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -66,6 +68,7 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -75,12 +78,15 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr ProgressCheckerSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr ProgressCheckerSelectorTestFixture:: +executor_ = nullptr; BT::NodeConfiguration * ProgressCheckerSelectorTestFixture::config_ = nullptr; std::shared_ptr ProgressCheckerSelectorTestFixture::factory_ = nullptr; @@ -129,7 +135,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); progress_checker_selector_pub->publish(selected_progress_checker_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check progress_checker updated @@ -179,7 +185,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); progress_checker_selector_pub->publish(selected_progress_checker_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check goal_checker updated diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index 50238346741..e099477de84 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -31,6 +31,8 @@ class SmootherSelectorTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("smoother_selector_test_fixture"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); // Configure and activate the lifecycle node node_->configure(); @@ -67,6 +69,7 @@ class SmootherSelectorTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); factory_.reset(); + executor_.reset(); } void TearDown() override @@ -76,12 +79,15 @@ class SmootherSelectorTestFixture : public ::testing::Test protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; nav2::LifecycleNode::SharedPtr SmootherSelectorTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr SmootherSelectorTestFixture::executor_ = + nullptr; BT::NodeConfiguration * SmootherSelectorTestFixture::config_ = nullptr; std::shared_ptr SmootherSelectorTestFixture::factory_ = nullptr; @@ -127,7 +133,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) tree_->rootNode()->executeTick(); smoother_selector_pub->publish(selected_smoother_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check smoother updated @@ -175,7 +181,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) tree_->rootNode()->executeTick(); smoother_selector_pub->publish(selected_smoother_cmd); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); } // check smoother updated diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp index fb27568c275..9733012e952 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp @@ -29,6 +29,8 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("test_is_battery_charging"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); factory_ = std::make_shared(); config_ = new BT::NodeConfiguration(); @@ -58,10 +60,12 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test battery_pub_.reset(); node_.reset(); factory_.reset(); + executor_.reset(); } protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static nav2::Publisher::SharedPtr @@ -69,6 +73,8 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test }; nav2::LifecycleNode::SharedPtr IsBatteryChargingConditionTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr IsBatteryChargingConditionTestFixture:: +executor_ = nullptr; BT::NodeConfiguration * IsBatteryChargingConditionTestFixture::config_ = nullptr; std::shared_ptr IsBatteryChargingConditionTestFixture::factory_ = nullptr; nav2::Publisher::SharedPtr @@ -90,32 +96,32 @@ TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status) battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); } diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index a5080593117..86affc16424 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -30,6 +30,8 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test static void SetUpTestCase() { node_ = std::make_shared("test_is_battery_low"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); factory_ = std::make_shared(); config_ = new BT::NodeConfiguration(); @@ -59,10 +61,12 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test battery_pub_.reset(); node_.reset(); factory_.reset(); + executor_.reset(); } protected: static nav2::LifecycleNode::SharedPtr node_; + static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static nav2::Publisher::SharedPtr @@ -70,6 +74,8 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test }; nav2::LifecycleNode::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr; +rclcpp::executors::SingleThreadedExecutor::SharedPtr IsBatteryLowConditionTestFixture:: +executor_ = nullptr; BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr; std::shared_ptr IsBatteryLowConditionTestFixture::factory_ = nullptr; nav2::Publisher::SharedPtr @@ -91,25 +97,25 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) battery_msg.percentage = 1.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.49; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.percentage = 0.51; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } @@ -129,25 +135,25 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) battery_msg.voltage = 10.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 4.9; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.voltage = 5.1; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(node_->get_node_base_interface()); + executor_->spin_some(); EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 3191de1727e..02fc680afe5 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -239,13 +239,6 @@ class Polygon rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( std::vector parameters); - /** - * @brief Checks if point is inside polygon - * @param point Given point to check - * @return True if given point is inside polygon, otherwise false - */ - bool isPointInside(const Point & point) const; - /** * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...] * @param poly_string Input String containing the verteceis of the polygon diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 5e13f5d5ef8..b37f677fb57 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -22,6 +22,7 @@ #include "tf2/transform_datatypes.hpp" #include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/array_parser.hpp" @@ -229,7 +230,7 @@ int Polygon::getPointsInside(const std::vector & points) const { int num = 0; for (const Point & point : points) { - if (isPointInside(point)) { + if (nav2_util::geometry_utils::isPointInsidePolygon(point.x, point.y, poly_)) { num++; } } @@ -602,38 +603,6 @@ void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr updatePolygon(msg); } -inline bool Polygon::isPointInside(const Point & point) const -{ - // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." - // Communications of the ACM 5.8 (1962): 434. - // Implementation of ray crossings algorithm for point in polygon task solving. - // Y coordinate is fixed. Moving the ray on X+ axis starting from given point. - // Odd number of intersections with polygon boundaries means the point is inside polygon. - const int poly_size = poly_.size(); - int i, j; // Polygon vertex iterators - bool res = false; // Final result, initialized with already inverted value - - // Starting from the edge where the last point of polygon is connected to the first - i = poly_size - 1; - for (j = 0; j < poly_size; j++) { - // Checking the edge only if given point is between edge boundaries by Y coordinates. - // One of the condition should contain equality in order to exclude the edges - // parallel to X+ ray. - if ((point.y <= poly_[i].y) == (point.y > poly_[j].y)) { - // Calculating the intersection coordinate of X+ ray - const double x_inter = poly_[i].x + - (point.y - poly_[i].y) * (poly_[j].x - poly_[i].x) / - (poly_[j].y - poly_[i].y); - // If intersection with checked edge is greater than point.x coordinate, inverting the result - if (x_inter > point.x) { - res = !res; - } - } - i = j; - } - return res; -} - bool Polygon::getPolygonFromString( std::string & poly_string, std::vector & polygon) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 4bbd37bc4e9..a49a28008f3 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -155,6 +155,7 @@ class Tester : public ::testing::Test protected: // CollisionDetector node std::shared_ptr cd_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; // Data source publishers nav2::Publisher::SharedPtr @@ -178,6 +179,8 @@ class Tester : public ::testing::Test Tester::Tester() { cd_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(cd_->get_node_base_interface()); scan_pub_ = cd_->create_publisher( SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -213,6 +216,7 @@ Tester::~Tester() collision_points_marker_sub_.reset(); cd_.reset(); + executor_.reset(); } bool Tester::waitState(const std::chrono::nanoseconds & timeout) @@ -222,7 +226,7 @@ bool Tester::waitState(const std::chrono::nanoseconds & timeout) if (state_msg_) { return true; } - rclcpp::spin_some(cd_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -236,7 +240,7 @@ bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) if (collision_points_marker_msg_) { return true; } - rclcpp::spin_some(cd_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -544,7 +548,7 @@ bool Tester::waitData( if (cd_->correctDataReceived(expected_dist, stamp)) { return true; } - rclcpp::spin_some(cd_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index ea0213f468c..92dd5e6751a 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -188,6 +188,7 @@ class Tester : public ::testing::Test // CollisionMonitor node std::shared_ptr cm_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; // Footprint publisher nav2::Publisher::SharedPtr @@ -222,6 +223,8 @@ class Tester : public ::testing::Test Tester::Tester() { cm_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(cm_->get_node_base_interface()); cm_->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false)); footprint_pub_ = cm_->create_publisher( @@ -281,6 +284,7 @@ Tester::~Tester() collision_points_marker_sub_.reset(); cm_.reset(); + executor_.reset(); } void Tester::setCommonParameters() @@ -724,7 +728,7 @@ bool Tester::waitData( if (cm_->correctDataReceived(expected_dist, stamp)) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -737,7 +741,7 @@ bool Tester::waitCmdVel(const std::chrono::nanoseconds & timeout) if (cmd_vel_out_) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -753,7 +757,7 @@ bool Tester::waitFuture( if (status == std::future_status::ready) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -766,7 +770,7 @@ bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) if (action_state_) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -779,7 +783,7 @@ bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) if (collision_points_marker_msg_) { return true; } - rclcpp::spin_some(cm_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 86f393febfa..bfe9d38f8a7 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -162,18 +162,9 @@ class TestNode : public nav2::LifecycleNode polygon_received_ = msg; } - geometry_msgs::msg::PolygonStamped::SharedPtr waitPolygonReceived( - const std::chrono::nanoseconds & timeout) + geometry_msgs::msg::PolygonStamped::SharedPtr getPolygonReceived() { - rclcpp::Time start_time = this->now(); - while (rclcpp::ok() && this->now() - start_time <= rclcpp::Duration(timeout)) { - if (polygon_received_) { - return polygon_received_; - } - rclcpp::spin_some(this->get_node_base_interface()); - std::this_thread::sleep_for(10ms); - } - return nullptr; + return polygon_received_; } private: @@ -265,6 +256,9 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout, std::vector & poly); + geometry_msgs::msg::PolygonStamped::SharedPtr waitPolygonReceived( + const std::chrono::nanoseconds & timeout); + // Wait until circle polygon radius will be received bool waitRadius(const std::chrono::nanoseconds & timeout); @@ -274,6 +268,7 @@ class Tester : public ::testing::Test std::vector & footprint); std::shared_ptr test_node_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::shared_ptr polygon_; std::shared_ptr circle_; @@ -285,6 +280,8 @@ class Tester : public ::testing::Test Tester::Tester() { test_node_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(test_node_->get_node_base_interface()); test_node_->configure(); test_node_->activate(); @@ -471,6 +468,21 @@ void Tester::sendTransforms(double shift) tf_broadcaster->sendTransform(transform); } +geometry_msgs::msg::PolygonStamped::SharedPtr Tester::waitPolygonReceived( + const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + auto polygon = test_node_->getPolygonReceived(); + if (polygon) { + return polygon; + } + executor_->spin_some(); + std::this_thread::sleep_for(10ms); + } + return nullptr; +} + bool Tester::waitPolygon( const std::chrono::nanoseconds & timeout, std::vector & poly) @@ -481,7 +493,7 @@ bool Tester::waitPolygon( if (poly.size() > 0) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -494,7 +506,7 @@ bool Tester::waitRadius(const std::chrono::nanoseconds & timeout) if (circle_->isShapeSet()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -512,7 +524,7 @@ bool Tester::waitFootprint( if (footprint.size() > 0) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -956,8 +968,7 @@ TEST_F(Tester, testPolygonPublish) { createPolygon("stop", true); polygon_->publish(); - geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received = - test_node_->waitPolygonReceived(500ms); + geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received = waitPolygonReceived(500ms); ASSERT_NE(polygon_received, nullptr); ASSERT_EQ(polygon_received->polygon.points.size(), 4u); @@ -996,7 +1007,7 @@ TEST_F(Tester, testPolygonDefaultVisualize) polygon_->publish(); // Wait for polygon: it should not be published - ASSERT_EQ(test_node_->waitPolygonReceived(100ms), nullptr); + ASSERT_EQ(waitPolygonReceived(100ms), nullptr); } TEST_F(Tester, testPolygonInvalidPointsString) diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 034cdd40d96..60a9f896761 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -375,6 +375,7 @@ class Tester : public ::testing::Test void checkPolygon(const std::vector & data); std::shared_ptr test_node_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::shared_ptr scan_; std::shared_ptr pointcloud_; std::shared_ptr range_; @@ -384,6 +385,8 @@ class Tester : public ::testing::Test Tester::Tester() { test_node_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(test_node_->get_node_base_interface()); tf_buffer_ = std::make_shared(test_node_->get_clock()); tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model @@ -506,7 +509,7 @@ bool Tester::waitScan(const std::chrono::nanoseconds & timeout) if (scan_->dataReceived()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -519,7 +522,7 @@ bool Tester::waitPointCloud(const std::chrono::nanoseconds & timeout) if (pointcloud_->dataReceived()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -532,7 +535,7 @@ bool Tester::waitRange(const std::chrono::nanoseconds & timeout) if (range_->dataReceived()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; @@ -545,7 +548,7 @@ bool Tester::waitPolygon(const std::chrono::nanoseconds & timeout) if (polygon_->dataReceived()) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 88024dd723b..64f318dbdbc 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -92,20 +92,6 @@ class TestNode : public nav2::LifecycleNode polygon_received_ = msg; } - geometry_msgs::msg::PolygonStamped::SharedPtr waitPolygonReceived( - const std::chrono::nanoseconds & timeout) - { - rclcpp::Time start_time = this->now(); - while (rclcpp::ok() && this->now() - start_time <= rclcpp::Duration(timeout)) { - if (polygon_received_) { - return polygon_received_; - } - rclcpp::spin_some(this->get_node_base_interface()); - std::this_thread::sleep_for(10ms); - } - return nullptr; - } - private: nav2::Subscription::SharedPtr polygon_sub_; geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received_; @@ -167,6 +153,7 @@ class Tester : public ::testing::Test std::vector & poly); std::shared_ptr test_node_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::shared_ptr velocity_polygon_; @@ -177,6 +164,8 @@ class Tester : public ::testing::Test Tester::Tester() { test_node_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node(test_node_->get_node_base_interface()); test_node_->configure(); test_node_->activate(); @@ -382,7 +371,7 @@ bool Tester::waitPolygon( if (poly.size() > 0) { return true; } - rclcpp::spin_some(test_node_->get_node_base_interface()); + executor_->spin_some(); std::this_thread::sleep_for(10ms); } return false; diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index 9e984bc6550..fdabbf28133 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -214,13 +214,13 @@ class BehaviorTreeNavigator : public NavigatorBase getName(), plugin_lib_names, default_bt_xml_filename, - search_directories, std::bind(&BehaviorTreeNavigator::onGoalReceived, this, std::placeholders::_1), std::bind(&BehaviorTreeNavigator::onLoop, this), std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1), std::bind( &BehaviorTreeNavigator::onCompletion, this, - std::placeholders::_1, std::placeholders::_2)); + std::placeholders::_1, std::placeholders::_2), + search_directories); bool ok = true; if (!bt_action_server_->on_configure()) { diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index fbacf15aa24..fe3764c443a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -461,105 +461,7 @@ class Costmap2D */ virtual void initMaps(unsigned int size_x, unsigned int size_y); - /** - * @brief Raytrace a line and apply some action at each step - * @param at The action to take... a functor - * @param x0 The starting x coordinate - * @param y0 The starting y coordinate - * @param x1 The ending x coordinate - * @param y1 The ending y coordinate - * @param max_length The maximum desired length of the segment... - * allows you to not go all the way to the endpoint - * @param min_length The minimum desired length of the segment - */ - template - inline void raytraceLine( - ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, - unsigned int y1, - unsigned int max_length = UINT_MAX, unsigned int min_length = 0) - { - int dx_full = x1 - x0; - int dy_full = y1 - y0; - - // we need to chose how much to scale our dominant dimension, - // based on the maximum length of the line - double dist = std::hypot(dx_full, dy_full); - if (dist < min_length) { - return; - } - - unsigned int min_x0, min_y0; - if (dist > 0.0) { - // Adjust starting point and offset to start from min_length distance - min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); - min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); - } else { - // dist can be 0 if [x0, y0]==[x1, y1]. - // In this case only this cell should be processed. - min_x0 = x0; - min_y0 = y0; - } - unsigned int offset = min_y0 * size_x_ + min_x0; - - int dx = x1 - min_x0; - int dy = y1 - min_y0; - - unsigned int abs_dx = abs(dx); - unsigned int abs_dy = abs(dy); - - int offset_dx = sign(dx); - int offset_dy = sign(dy) * size_x_; - - double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); - // if x is dominant - if (abs_dx >= abs_dy) { - int error_y = abs_dx / 2; - - bresenham2D( - at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); - return; - } - - // otherwise y is dominant - int error_x = abs_dy / 2; - - bresenham2D( - at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); - } - private: - /** - * @brief A 2D implementation of Bresenham's raytracing algorithm... - * applies an action at each step - */ - template - inline void bresenham2D( - ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, - int offset_a, - int offset_b, unsigned int offset, - unsigned int max_length) - { - unsigned int end = std::min(max_length, abs_da); - for (unsigned int i = 0; i < end; ++i) { - at(offset); - offset += offset_a; - error_b += abs_db; - if ((unsigned int)error_b >= abs_da) { - offset += offset_b; - error_b -= abs_da; - } - } - at(offset); - } - - /** - * @brief get the sign of an int - */ - inline int sign(int x) - { - return x > 0 ? 1.0 : -1.0; - } - mutex_t * access_; protected: diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index 25238cf8a35..da4720ff798 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -185,20 +185,6 @@ class CostmapFilter : public Layer const std::string mask_frame, geometry_msgs::msg::Pose & mask_pose) const; - /** - * @brief: Convert from world coordinates to mask coordinates. - Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. - * @param filter_mask Filter mask on which to convert - * @param wx The x world coordinate - * @param wy The y world coordinate - * @param mx Will be set to the associated mask x coordinate - * @param my Will be set to the associated mask y coordinate - * @return True if the conversion was successful (legal bounds) false otherwise - */ - bool worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const; - /** * @brief Get the data of a cell in the filter mask * @param filter_mask Filter mask to get the data from diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp index 097537d40c6..c805a537396 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -44,6 +44,7 @@ #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -190,8 +191,8 @@ void BinaryFilter::process( // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i, - mask_robot_j)) + if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y, + mask_robot_i, mask_robot_j)) { // Robot went out of mask range. Set "false" state by-default RCLCPP_WARN( diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index 06d7ed2d025..58b89740b8b 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -193,29 +193,6 @@ bool CostmapFilter::transformPose( return true; } -bool CostmapFilter::worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const -{ - const double origin_x = filter_mask->info.origin.position.x; - const double origin_y = filter_mask->info.origin.position.y; - const double resolution = filter_mask->info.resolution; - const unsigned int size_x = filter_mask->info.width; - const unsigned int size_y = filter_mask->info.height; - - if (wx < origin_x || wy < origin_y) { - return false; - } - - mx = static_cast((wx - origin_x) / resolution); - my = static_cast((wy - origin_y) / resolution); - if (mx >= size_x || my >= size_y) { - return false; - } - - return true; -} - unsigned char CostmapFilter::getMaskCost( nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int & my) const diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 48cd6c74e42..c432a917e59 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -35,15 +35,17 @@ * Author: Alexey Merzlyakov *********************************************************************/ +#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" + #include #include #include #include "tf2/convert.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -200,8 +202,8 @@ void KeepoutFilter::updateBounds( geometry_msgs::msg::Pose mask_pose; if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) { unsigned int mask_robot_i, mask_robot_j; - if (worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i, - mask_robot_j)) + if (nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y, + mask_robot_i, mask_robot_j)) { auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j); is_pose_lethal_ = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE); @@ -366,7 +368,7 @@ void KeepoutFilter::process( msk_wy = gl_wy; } // Get mask coordinates corresponding to (i, j) point at filter_mask_ - if (worldToMask(filter_mask_, msk_wx, msk_wy, mx, my)) { + if (nav2_util::worldToMap(filter_mask_, msk_wx, msk_wy, mx, my)) { data = getMaskCost(filter_mask_, mx, my); // Update if mask_ data is valid and greater than existing master_grid's one if (data == NO_INFORMATION) { diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index a66886bc6d0..c1e822cb03a 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -43,6 +43,7 @@ #include #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -198,8 +199,8 @@ void SpeedFilter::process( // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i, - mask_robot_j)) + if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y, + mask_robot_i, mask_robot_j)) { return; } diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index b897ac515be..c09d60da61b 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -45,7 +45,9 @@ #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "nav2_util/raytrace_line_2d.hpp" #include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "rclcpp/version.h" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) @@ -768,7 +770,8 @@ ObstacleLayer::raytraceFreespace( unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_); MarkCell marker(costmap_, FREE_SPACE); // and finally... we can execute our trace to clear obstacles along that line - raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_max_range, cell_raytrace_min_range); + nav2_util::raytraceLine( + marker, x0, y0, x1, y1, size_x_, cell_raytrace_max_range, cell_raytrace_min_range); updateRaytraceBounds( ox, oy, wx, wy, clearing_observation.raytrace_max_range_, diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index f3903308cff..28798b0f8a3 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -43,6 +43,7 @@ #include #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/raytrace_line_2d.hpp" namespace nav2_costmap_2d { @@ -462,14 +463,15 @@ void Costmap2D::polygonOutlineCells( { PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells); for (unsigned int i = 0; i < polygon.size() - 1; ++i) { - raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y); + nav2_util::raytraceLine( + cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y, size_x_); } if (!polygon.empty()) { unsigned int last_index = polygon.size() - 1; // we also need to close the polygon by going from the last point to the first - raytraceLine( + nav2_util::raytraceLine( cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, - polygon[0].y); + polygon[0].y, size_x_); } } diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp index 94e0f72ea77..684aaa58a12 100644 --- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp +++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp @@ -32,6 +32,10 @@ TEST(DynParamTestNode, testDynParamsSet) { auto node = std::make_shared("dyn_param_tester"); auto costmap = std::make_shared("test_costmap"); + rclcpp::executors::SingleThreadedExecutor node_executor; + node_executor.add_node(node->get_node_base_interface()); + rclcpp::executors::SingleThreadedExecutor costmap_executor; + costmap_executor.add_node(costmap->get_node_base_interface()); costmap->on_configure(rclcpp_lifecycle::State()); // Set tf between default global_frame and robot_base_frame in order not to block in on_activate @@ -76,8 +80,8 @@ TEST(DynParamTestNode, testDynParamsSet) rclcpp::Parameter("robot_base_frame", "wrong_test_frame"), }); - rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); - rclcpp::spin_all(costmap->get_node_base_interface(), std::chrono::milliseconds(50)); + node_executor.spin_all(std::chrono::milliseconds(50)); + costmap_executor.spin_all(std::chrono::milliseconds(50)); EXPECT_EQ(costmap->get_parameter("robot_radius").as_double(), 1.234); EXPECT_EQ(costmap->get_parameter("footprint_padding").as_double(), 2.345); diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 19446c00e38..658791beeb5 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -102,8 +102,10 @@ std::vector TestNode::setRadii( void TestNode::waitForMap(std::shared_ptr & slayer) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); while (!slayer->isCurrent()) { - rclcpp::spin_some(node_->get_node_base_interface()); + executor.spin_some(); } } diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp index 13969ae9a55..dc8a4f4b3a2 100644 --- a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp +++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp @@ -93,8 +93,10 @@ class TestNode : public ::testing::Test void waitForMap(std::shared_ptr & slayer) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); while (!slayer->isCurrent()) { - rclcpp::spin_some(node_->get_node_base_interface()); + executor.spin_some(); } } diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index 85468400224..4921fa38bb5 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -205,6 +205,8 @@ TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) auto costmapPublisher = std::make_shared( node, costmapToSend.get(), "", topicName, always_send_full_costmap); costmapPublisher->on_activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); for (const auto & mapChange : mapChanges) { for (const auto & observation : mapChange.observations) { @@ -220,7 +222,7 @@ TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) expectedGrids.emplace_back(grid); costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); costmapPublisher->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); receivedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); } @@ -252,6 +254,9 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) std::uint32_t yn = costmapToSend->getSizeInCellsY(); bool first_iteration = true; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + for (const auto & mapChange : mapChanges) { for (const auto & observation : mapChange.observations) { costmapToSend->setCost(observation.x, observation.y, observation.cost); @@ -272,7 +277,7 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) expectedGrids.emplace_back(grid); costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); costmapPublisher->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); receivedCostmaps.emplace_back(getUpdatedCharMapFromSubscriber(x0, xn, y0, yn)); first_iteration = false; } diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index e5b73e4b5cc..d7da1e104c5 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -133,9 +133,10 @@ class TestCollisionChecker : public nav2::LifecycleNode // Add Static Layer std::shared_ptr slayer = nullptr; addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer, callback_group_); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(this->get_node_base_interface()); while (!slayer->isCurrent()) { - rclcpp::spin_some(this->get_node_base_interface()); + executor.spin_some(); } // Add Inflation Layer std::shared_ptr ilayer = nullptr; diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index d468dd37987..435d9072d03 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,9 +1,3 @@ -# Bresenham2D corner cases test -ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) -target_link_libraries(costmap_bresenham_2d - nav2_costmap_2d_core -) - # OrderLayer for checking Costmap2D plugins API calling order add_library(order_layer SHARED order_layer.cpp) target_link_libraries(order_layer PUBLIC diff --git a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp deleted file mode 100644 index c4afee34e00..00000000000 --- a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2022 Samsung Research Russia -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Alexey Merzlyakov -*********************************************************************/ -#include -#include - -class CostmapAction -{ -public: - explicit CostmapAction( - unsigned char * costmap, unsigned int size, unsigned char mark_val = 128) - : costmap_(costmap), size_(size), mark_val_(mark_val) - { - } - - inline void operator()(unsigned int off) - { - ASSERT_TRUE(off < size_); - costmap_[off] = mark_val_; - } - - inline unsigned int get(unsigned int off) - { - return costmap_[off]; - } - -private: - unsigned char * costmap_; - unsigned int size_; - unsigned char mark_val_; -}; - -class CostmapTest : public nav2_costmap_2d::Costmap2D -{ -public: - CostmapTest( - unsigned int size_x, unsigned int size_y, double resolution, - double origin_x, double origin_y, unsigned char default_val = 0) - : nav2_costmap_2d::Costmap2D(size_x, size_y, resolution, origin_x, origin_y, default_val) - { - } - - unsigned char * getCostmap() - { - return costmap_; - } - - unsigned int getSize() - { - return size_x_ * size_y_; - } - - void raytraceLine( - CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1, - unsigned int y1, - unsigned int max_length = UINT_MAX, unsigned int min_length = 0) - { - nav2_costmap_2d::Costmap2D::raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } -}; - -TEST(costmap_2d, bresenham2DBoundariesCheck) -{ - const unsigned int sz_x = 60; - const unsigned int sz_y = 60; - const unsigned int max_length = 60; - const unsigned int min_length = 6; - CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); - CostmapAction ca(ct.getCostmap(), ct.getSize()); - - // Initial point - some asymmetrically standing point in order to cover most corner cases - const unsigned int x0 = 2; - const unsigned int y0 = 4; - // (x1, y1) point will move - unsigned int x1, y1; - - // Running on (x, 0) edge - y1 = 0; - for (x1 = 0; x1 < sz_x; x1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (x, sz_y) edge - y1 = sz_y - 1; - for (x1 = 0; x1 < sz_x; x1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (0, y) edge - x1 = 0; - for (y1 = 0; y1 < sz_y; y1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (sz_x, y) edge - x1 = sz_x - 1; - for (y1 = 0; y1 < sz_y; y1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } -} - -TEST(costmap_2d, bresenham2DSamePoint) -{ - const unsigned int sz_x = 60; - const unsigned int sz_y = 60; - const unsigned int max_length = 60; - const unsigned int min_length = 0; - CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); - CostmapAction ca(ct.getCostmap(), ct.getSize()); - - // Initial point - const double x0 = 2; - const double y0 = 4; - - unsigned int offset = y0 * sz_x + x0; - unsigned char val_before = ca.get(offset); - // Same point to check - ct.raytraceLine(ca, x0, y0, x0, y0, max_length, min_length); - unsigned char val_after = ca.get(offset); - ASSERT_FALSE(val_before == val_after); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index e0061e91e88..bfce85d2106 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -251,6 +251,7 @@ class TestNode : public ::testing::Test const double resolution_ = 1.0; nav2::LifecycleNode::SharedPtr node_; + rclcpp::executors::SingleThreadedExecutor node_executor_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -262,6 +263,7 @@ class TestNode : public ::testing::Test std::shared_ptr info_publisher_; std::shared_ptr mask_publisher_; std::shared_ptr binary_state_subscriber_; + rclcpp::executors::SingleThreadedExecutor binary_state_subscriber_executor_; }; void TestNode::createMaps(const std::string & mask_frame) @@ -318,8 +320,8 @@ void TestNode::waitSome(const std::chrono::nanoseconds & duration) { rclcpp::Time start_time = node_->now(); while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { - rclcpp::spin_some(node_->get_node_base_interface()); - rclcpp::spin_some(binary_state_subscriber_); + node_executor_.spin_some(); + binary_state_subscriber_executor_.spin_some(); std::this_thread::sleep_for(10ms); } } @@ -327,7 +329,7 @@ void TestNode::waitSome(const std::chrono::nanoseconds & duration) std_msgs::msg::Bool::SharedPtr TestNode::getBinaryState() { std::this_thread::sleep_for(100ms); - rclcpp::spin_some(binary_state_subscriber_); + binary_state_subscriber_executor_.spin_some(); return binary_state_subscriber_->getBinaryState(); } @@ -342,7 +344,7 @@ std_msgs::msg::Bool::SharedPtr TestNode::waitBinaryState() binary_state_subscriber_->resetBinaryStateIndicator(); return binary_state_subscriber_->getBinaryState(); } - rclcpp::spin_some(binary_state_subscriber_); + binary_state_subscriber_executor_.spin_some(); std::this_thread::sleep_for(10ms); } return nullptr; @@ -389,6 +391,8 @@ bool TestNode::createBinaryFilter(const std::string & global_frame, double flip_ binary_state_subscriber_ = std::make_shared(BINARY_STATE_TOPIC, default_state_); + binary_state_subscriber_executor_.add_node(binary_state_subscriber_); + node_executor_.add_node(node_->get_node_base_interface()); // Wait until mask will be received by BinaryFilter const std::chrono::nanoseconds timeout = 500ms; @@ -397,7 +401,7 @@ bool TestNode::createBinaryFilter(const std::string & global_frame, double flip_ if (node_->now() - start_time > rclcpp::Duration(timeout)) { return false; } - rclcpp::spin_some(node_->get_node_base_interface()); + node_executor_.spin_some(); std::this_thread::sleep_for(10ms); } return true; diff --git a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp index 320eeaa1653..8cf4583d15d 100644 --- a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp @@ -19,6 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" @@ -30,13 +31,6 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter public: CostmapFilterWrapper() {} - bool worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const - { - return nav2_costmap_2d::CostmapFilter::worldToMask(filter_mask, wx, wy, mx, my); - } - unsigned char getMaskCost( nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int & my) const @@ -52,7 +46,7 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter void resetFilter() {} }; -TEST(CostmapFilter, testWorldToMask) +TEST(CostmapFilter, testWorldToMap) { // Create occupancy grid for test as follows: // @@ -82,19 +76,19 @@ TEST(CostmapFilter, testWorldToMask) CostmapFilterWrapper cf; unsigned int mx, my; // Point inside mask - ASSERT_TRUE(cf.worldToMask(mask, 4.0, 5.0, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 4.0, 5.0, mx, my)); ASSERT_EQ(mx, 1u); ASSERT_EQ(my, 2u); // Corner cases - ASSERT_TRUE(cf.worldToMask(mask, 3.0, 3.0, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 3.0, 3.0, mx, my)); ASSERT_EQ(mx, 0u); ASSERT_EQ(my, 0u); - ASSERT_TRUE(cf.worldToMask(mask, 5.9, 5.9, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 5.9, 5.9, mx, my)); ASSERT_EQ(mx, 2u); ASSERT_EQ(my, 2u); // Point outside mask - ASSERT_FALSE(cf.worldToMask(mask, 2.9, 2.9, mx, my)); - ASSERT_FALSE(cf.worldToMask(mask, 6.0, 6.0, mx, my)); + ASSERT_FALSE(nav2_util::worldToMap(mask, 2.9, 2.9, mx, my)); + ASSERT_FALSE(nav2_util::worldToMap(mask, 6.0, 6.0, mx, my)); } TEST(CostmapFilter, testGetMaskCost) diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index 37a5a683299..f6078c52cc0 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -200,8 +200,10 @@ void TestNode::rePublishMask() void TestNode::waitSome(const std::chrono::nanoseconds & duration) { rclcpp::Time start_time = node_->now(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { - rclcpp::spin_some(node_->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(10ms); } } @@ -229,8 +231,10 @@ void TestNode::createKeepoutFilter(const std::string & global_frame) keepout_filter_->initializeFilter(INFO_TOPIC); // Wait until mask will be received by KeepoutFilter + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); while (!keepout_filter_->isActive()) { - rclcpp::spin_some(node_->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(10ms); } } diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index 5d3eebb6db5..aabe7ce03b7 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -244,6 +244,7 @@ class TestNode : public ::testing::Test const double resolution_ = 1.0; nav2::LifecycleNode::SharedPtr node_; + rclcpp::executors::SingleThreadedExecutor node_executor_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -255,6 +256,7 @@ class TestNode : public ::testing::Test std::shared_ptr info_publisher_; std::shared_ptr mask_publisher_; std::shared_ptr speed_limit_subscriber_; + rclcpp::executors::SingleThreadedExecutor speed_limit_subscriber_executor_; }; void TestNode::createMaps(const std::string & mask_frame) @@ -308,7 +310,7 @@ void TestNode::rePublishMask() nav2_msgs::msg::SpeedLimit::SharedPtr TestNode::getSpeedLimit() { std::this_thread::sleep_for(100ms); - rclcpp::spin_some(speed_limit_subscriber_); + speed_limit_subscriber_executor_.spin_some(); return speed_limit_subscriber_->getSpeedLimit(); } @@ -323,7 +325,7 @@ nav2_msgs::msg::SpeedLimit::SharedPtr TestNode::waitSpeedLimit() speed_limit_subscriber_->resetSpeedLimitIndicator(); return speed_limit_subscriber_->getSpeedLimit(); } - rclcpp::spin_some(speed_limit_subscriber_); + speed_limit_subscriber_executor_.spin_some(); std::this_thread::sleep_for(10ms); } return nullptr; @@ -333,8 +335,8 @@ void TestNode::waitSome(const std::chrono::nanoseconds & duration) { rclcpp::Time start_time = node_->now(); while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { - rclcpp::spin_some(node_->get_node_base_interface()); - rclcpp::spin_some(speed_limit_subscriber_); + node_executor_.spin_some(); + speed_limit_subscriber_executor_.spin_some(); std::this_thread::sleep_for(10ms); } } @@ -366,6 +368,8 @@ bool TestNode::createSpeedFilter(const std::string & global_frame) speed_filter_->initializeFilter(INFO_TOPIC); speed_limit_subscriber_ = std::make_shared(SPEED_LIMIT_TOPIC); + speed_limit_subscriber_executor_.add_node(speed_limit_subscriber_); + node_executor_.add_node(node_->get_node_base_interface()); // Wait until mask will be received by SpeedFilter const std::chrono::nanoseconds timeout = 500ms; @@ -374,7 +378,7 @@ bool TestNode::createSpeedFilter(const std::string & global_frame) if (node_->now() - start_time > rclcpp::Duration(timeout)) { return false; } - rclcpp::spin_some(node_->get_node_base_interface()); + node_executor_.spin_some(); std::this_thread::sleep_for(10ms); } return true; diff --git a/nav2_docking/README.md b/nav2_docking/README.md index f246c0c2493..2d2d92d8f97 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -36,12 +36,14 @@ The `ChargingDock` and `NonChargingDock` plugins are the heart of the customizab The docking procedure is as follows: 1. Take action request and obtain the dock's plugin and its pose 2. If the robot is not within the prestaging tolerance of the dock's staging pose, navigate to the staging pose -3. Use the dock's plugin to initially detect the dock and return the docking pose -4. Enter a vision-control loop where the robot attempts to reach the docking pose while its actively being refined by the vision system -5. Exit the vision-control loop once contact has been detected or charging has started -6. Wait until charging starts (if applicable) and return success. +3. Call the dock's plugin `startDetectionProcess()` method to activate any external detection mechanisms. +4. Use the dock's plugin to initially detect the dock (`getRefinedPose`) and return the docking pose. +5. Enter a vision-control loop where the robot attempts to reach the docking pose while it's actively being refined by the vision system. +6. Exit the vision-control loop once contact has been detected or charging has started (if applicable). +7. Wait until charging starts (if applicable) and return success. +8. Call the dock's plugin `stopDetectionProcess()` method to deactivate any external detection mechanisms. -If anywhere this procedure is unsuccessful, `N` retries may be made by driving back to the dock's staging pose and trying again. If still unsuccessful, it will return a failure code to indicate what kind of failure occurred to the client. +If anywhere this procedure is unsuccessful (before step 8), `N` retries may be made, driving back to the dock's staging pose, and then restarting the process from step 3. If still unsuccessful after retries, it will return a failure code to indicate what kind of failure occurred to the client. Undocking works more simply: 1. If previously docked, use the known dock information to get the dock type. If not, use the undock action request's indicated dock type @@ -175,8 +177,9 @@ some robots. `getStagingPose` applies a parameterized translational and rotational offset to the dock pose to obtain the staging pose. `getRefinedPose` can be used in two ways. -1. A blind approach where the returned dock pose will simply be equal to whatever was passed in from the dock database. This may work with a reduced success rate on a real robot (due to global localization error), but is useful for initial testing and simulation. +1. A blind approach where the returned dock pose will simply be equal to whatever was passed in from the dock database. This may work with a reduced success rate on a real robot (due to global localization error), but is useful for initial testing and simulation. The `start/stopDetectionProcess` methods would typically do nothing in this case. 2. The more realistic use case is to use an AR marker, dock pose detection algorithm, etc. The plugin will subscribe to a `geometry_msgs/PoseStamped` topic `detected_dock_pose`. This can be used with the `image_proc/TrackMarkerNode` for Apriltags or other custom detectors for your dock. It is unlikely the detected pose is actually the pose you want to dock with, so several parameters are supplied to represent your docked pose relative to the detected feature's pose. +The subscription to `detected_dock_pose` can be managed dynamically (see `subscribe_toggle` parameter). Additionally, an external detector process can be triggered via a service call (see `detector_service_name`). The `DockingServer` calls `startDetectionProcess()` before `getRefinedPose` (which is called in a loop) and `stopDetectionProcess()` after the docking action concludes (successfully or not). This allows plugins to manage resources like GPU usage by only running detection when needed. During the docking approach, there are two options for detecting `isDocked`: 1. We can check the joint states of the wheels if the current has spiked above a set threshold to indicate that the robot has made contact with the dock or other physical object. @@ -257,6 +260,9 @@ Note: `dock_plugins` and either `docks` or `dock_database` are required. | staging_yaw_offset | Staging pose angle relative to dock pose (rad) | double | 0.0 | | dock_direction | Whether the robot is docking with the dock forward or backward in motion | string | "forward" or "backward" | | rotate_to_dock | Enables backward docking without requiring a sensor for detection during the final approach. When enabled, the robot approaches the staging pose facing forward with sensor coverage for dock detection; after detection, it rotates and backs into the dock using only the initially detected pose for dead reckoning. | bool | false | +| detector_service_name | Trigger service name to start/stop the detector (e.g., a camera node or an AprilTag detector node). Leave empty to disable service calls. | string | "" | +| detector_service_timeout | Timeout (seconds) to wait for the detector service (if `detector_service_name` is set) to become available or respond. | double | 5.0 | +| subscribe_toggle | If true, dynamically subscribe/unsubscribe to `detected_dock_pose` topic when `start/stopDetectionProcess` are called. If false, the subscription is kept alive throughout the plugin's lifecycle if `use_external_detection_pose` is true. This can be useful if the detector node is always running and publishing. | bool | false | Note: The external detection rotation angles are setup to work out of the box with Apriltags detectors in `image_proc` and `isaac_ros`. diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index 8801b18d5a8..cbb068aa504 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -127,6 +128,7 @@ target_link_libraries(simple_charging_dock PUBLIC rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ${sensor_msgs_TARGETS} + ${std_srvs_TARGETS} tf2_geometry_msgs::tf2_geometry_msgs tf2_ros::tf2_ros nav2_ros_common::nav2_ros_common @@ -155,6 +157,7 @@ target_link_libraries(simple_non_charging_dock PUBLIC rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ${sensor_msgs_TARGETS} + ${std_srvs_TARGETS} tf2_geometry_msgs::tf2_geometry_msgs tf2_ros::tf2_ros ) @@ -211,6 +214,7 @@ ament_export_dependencies( rclcpp_action rclcpp_lifecycle rcl_interfaces + std_srvs tf2_ros yaml-cpp ) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp index 719e6e5cf7d..7cc33d34c4e 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp @@ -19,6 +19,7 @@ #include #include +#include "std_srvs/srv/trigger.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/joint_state.hpp" @@ -54,17 +55,17 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock /** * @brief Method to cleanup resources used on shutdown. */ - virtual void cleanup() {} + void cleanup() override; /** * @brief Method to active Behavior and any threads involved in execution. */ - virtual void activate() {} + void activate() override; /** * @brief Method to deactivate Behavior and any threads involved in execution. */ - virtual void deactivate() {} + void deactivate() override; /** * @brief Method to obtain the dock's staging pose. This method should likely @@ -104,14 +105,24 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock */ virtual bool hasStoppedCharging(); + /** + * @brief Start external detection process (service call + subscribe). + */ + bool startDetectionProcess() override; + + /** + * @brief Stop external detection process. + */ + bool stopDetectionProcess() override; + protected: void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state); // Optionally subscribe to a detected dock pose topic nav2::Subscription::SharedPtr dock_pose_sub_; - rclcpp::Publisher::SharedPtr dock_pose_pub_; - rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; - rclcpp::Publisher::SharedPtr staging_pose_pub_; + nav2::Publisher::SharedPtr dock_pose_pub_; + nav2::Publisher::SharedPtr filtered_dock_pose_pub_; + nav2::Publisher::SharedPtr staging_pose_pub_; // If subscribed to a detected pose topic, will contain latest message geometry_msgs::msg::PoseStamped detected_dock_pose_; // This is the actual dock pose once it has the specified translation/rotation applied @@ -150,6 +161,18 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock nav2::LifecycleNode::SharedPtr node_; std::shared_ptr tf2_buffer_; + + // Detector control parameters + std::string detector_service_name_; + double detector_service_timeout_{5.0}; + bool subscribe_toggle_{false}; + + // Client used to call the Trigger service + nav2::ServiceClient::SharedPtr detector_client_; + + // Detection state flags + bool detection_active_{false}; + bool initial_pose_received_{false}; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp index a0b3a46531e..ef2c7082e2c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp @@ -19,6 +19,7 @@ #include #include +#include "std_srvs/srv/trigger.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/joint_state.hpp" @@ -54,17 +55,17 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock /** * @brief Method to cleanup resources used on shutdown. */ - virtual void cleanup() {} + void cleanup() override; - /** - * @brief Method to active Behavior and any threads involved in execution. - */ - virtual void activate() {} + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + void activate() override; - /** - * @brief Method to deactivate Behavior and any threads involved in execution. - */ - virtual void deactivate() {} + /** + * @brief Method to deactivate Behavior and any threads involved in execution. + */ + void deactivate() override; /** * @brief Method to obtain the dock's staging pose. This method should likely @@ -89,14 +90,24 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock */ virtual bool isDocked(); + /** + * @brief Start external detection process (service call + subscribe). + */ + bool startDetectionProcess() override; + + /** + * @brief Stop external detection process. + */ + bool stopDetectionProcess() override; + protected: void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state); // Optionally subscribe to a detected dock pose topic nav2::Subscription::SharedPtr dock_pose_sub_; - rclcpp::Publisher::SharedPtr dock_pose_pub_; - rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; - rclcpp::Publisher::SharedPtr staging_pose_pub_; + nav2::Publisher::SharedPtr dock_pose_pub_; + nav2::Publisher::SharedPtr filtered_dock_pose_pub_; + nav2::Publisher::SharedPtr staging_pose_pub_; // If subscribed to a detected pose topic, will contain latest message geometry_msgs::msg::PoseStamped detected_dock_pose_; // This is the actual dock pose once it has the specified translation/rotation applied @@ -128,6 +139,18 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock nav2::LifecycleNode::SharedPtr node_; std::shared_ptr tf2_buffer_; + + // Detector control parameters + std::string detector_service_name_; + double detector_service_timeout_{5.0}; + bool subscribe_toggle_{false}; + + // Client used to call the Trigger service + nav2::ServiceClient::SharedPtr detector_client_; + + // Detection state flags + bool detection_active_{false}; + bool initial_pose_received_{false}; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index b65cb91ee56..97edbafb954 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -25,6 +25,7 @@ rclcpp_lifecycle rcl_interfaces sensor_msgs + std_srvs tf2 tf2_geometry_msgs tf2_ros diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 1208ade5521..ec7a7fc5e81 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -331,6 +331,7 @@ void DockingServer::dockRobot() result->num_retries = num_retries_; stashDockData(goal->use_dock_id, dock, true); publishZeroVelocity(); + dock->plugin->stopDetectionProcess(); docking_action_server_->succeeded_current(result); return; } @@ -339,6 +340,7 @@ void DockingServer::dockRobot() // Cancelled, preempted, or shutting down (recoverable errors throw DockingException) stashDockData(goal->use_dock_id, dock, false); publishZeroVelocity(); + dock->plugin->stopDetectionProcess(); docking_action_server_->terminate_all(result); return; } catch (opennav_docking_core::DockingException & e) { @@ -354,6 +356,7 @@ void DockingServer::dockRobot() // Cancelled, preempted, or shutting down stashDockData(goal->use_dock_id, dock, false); publishZeroVelocity(); + dock->plugin->stopDetectionProcess(); docking_action_server_->terminate_all(result); return; } @@ -401,6 +404,7 @@ void DockingServer::dockRobot() stashDockData(goal->use_dock_id, dock, false); result->num_retries = num_retries_; publishZeroVelocity(); + dock->plugin->stopDetectionProcess(); docking_action_server_->terminate_current(result); } @@ -429,12 +433,18 @@ Dock * DockingServer::generateGoalDock(std::shared_ptr go void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose) { publishDockingFeedback(DockRobot::Feedback::INITIAL_PERCEPTION); + + if (!dock->plugin->startDetectionProcess()) { + throw opennav_docking_core::FailedToDetectDock("Failed to start the detection process."); + } + rclcpp::Rate loop_rate(controller_frequency_); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(initial_perception_timeout_); while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) { if (this->now() - start > timeout) { - throw opennav_docking_core::FailedToDetectDock("Failed initial dock detection"); + throw opennav_docking_core::FailedToDetectDock( + "Failed initial dock detection: Timeout exceeded"); } if (checkAndWarnIfCancelled(docking_action_server_, "dock_robot") || diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index ff8be965603..3be3428e4eb 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -13,11 +13,14 @@ // limitations under the License. #include +#include #include "nav2_ros_common/node_utils.hpp" #include "opennav_docking/simple_charging_dock.hpp" #include "opennav_docking/utils.hpp" +using namespace std::chrono_literals; + namespace opennav_docking { @@ -37,6 +40,14 @@ void SimpleChargingDock::configure( nav2::declare_parameter_if_not_declared( node_, name + ".use_battery_status", rclcpp::ParameterValue(true)); + // Parameters for optional detector control + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_name", rclcpp::ParameterValue("")); + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_timeout", rclcpp::ParameterValue(5.0)); + nav2::declare_parameter_if_not_declared( + node_, name + ".subscribe_toggle", rclcpp::ParameterValue(false)); + // Parameters for optional external detection of dock pose nav2::declare_parameter_if_not_declared( node_, name + ".use_external_detection_pose", rclcpp::ParameterValue(false)); @@ -105,6 +116,26 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + node_->get_parameter(name + ".detector_service_name", detector_service_name_); + node_->get_parameter(name + ".detector_service_timeout", detector_service_timeout_); + node_->get_parameter(name + ".subscribe_toggle", subscribe_toggle_); + + // Initialize detection state + detection_active_ = false; + initial_pose_received_ = false; + + // Create persistent subscription if toggling is disabled. + if (use_external_detection_pose_ && !subscribe_toggle_) { + dock_pose_.header.stamp = rclcpp::Time(0); + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + std::string dock_direction; node_->get_parameter(name + ".dock_direction", dock_direction); dock_direction_ = utils::getDockDirectionFromString(dock_direction); @@ -123,21 +154,17 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".filter_coef", filter_coef); filter_ = std::make_unique(filter_coef, external_detection_timeout_); + if (!detector_service_name_.empty()) { + detector_client_ = node_->create_client( + detector_service_name_, false); + } + if (use_battery_status_) { battery_sub_ = node_->create_subscription( "battery_state", [this](const sensor_msgs::msg::BatteryState::SharedPtr state) { is_charging_ = state->current > charging_threshold_; - }, nav2::qos::StandardTopicQoS()); - } - - if (use_external_detection_pose_) { - dock_pose_.header.stamp = rclcpp::Time(0); - dock_pose_sub_ = node_->create_subscription( - "detected_dock_pose", - [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { - detected_dock_pose_ = *pose; - }, nav2::qos::StandardTopicQoS(1)); // Only want the most recent pose + }); } bool use_stall_detection; @@ -197,6 +224,12 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, return true; } + // Guard against using pose data before the first detection has arrived. + if (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Waiting for first detected_dock_pose; none received yet"); + return false; + } + // If using detections, get current detections, transform to frame, and apply offsets geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; @@ -323,6 +356,120 @@ void SimpleChargingDock::jointStateCallback(const sensor_msgs::msg::JointState:: is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_); } +bool SimpleChargingDock::startDetectionProcess() +{ + // Skip if already active + if (detection_active_) { + return true; + } + + // 1. Service START request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to start.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Subscription toggle + // Only subscribe once; will set state to ON on first message + if (subscribe_toggle_ && !dock_pose_sub_) { + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + + detection_active_ = true; + RCLCPP_INFO(node_->get_logger(), "External detector activation requested."); + return true; +} + +bool SimpleChargingDock::stopDetectionProcess() +{ + // Skip if already OFF + if (!detection_active_) { + return true; + } + + // 1. Service STOP request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to stop.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Unsubscribe to release resources + // reset() will tear down the topic subscription immediately + if (subscribe_toggle_ && dock_pose_sub_) { + dock_pose_sub_.reset(); + } + + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_INFO(node_->get_logger(), "External detector deactivation requested."); + return true; +} + +void SimpleChargingDock::activate() +{ + dock_pose_pub_->on_activate(); + filtered_dock_pose_pub_->on_activate(); + staging_pose_pub_->on_activate(); +} + +void SimpleChargingDock::deactivate() +{ + stopDetectionProcess(); + dock_pose_pub_->on_deactivate(); + filtered_dock_pose_pub_->on_deactivate(); + staging_pose_pub_->on_deactivate(); + RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock deactivated"); +} + +void SimpleChargingDock::cleanup() +{ + detector_client_.reset(); + dock_pose_sub_.reset(); + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock cleaned up"); +} + } // namespace opennav_docking #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp index 2a845559c6d..7e2aa26cf85 100644 --- a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -13,11 +13,14 @@ // limitations under the License. #include +#include #include "nav2_ros_common/node_utils.hpp" #include "opennav_docking/simple_non_charging_dock.hpp" #include "opennav_docking/utils.hpp" +using namespace std::chrono_literals; + namespace opennav_docking { @@ -50,6 +53,14 @@ void SimpleNonChargingDock::configure( nav2::declare_parameter_if_not_declared( node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); + // Parameters for optional detector control + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_name", rclcpp::ParameterValue("")); + nav2::declare_parameter_if_not_declared( + node_, name + ".detector_service_timeout", rclcpp::ParameterValue(5.0)); + nav2::declare_parameter_if_not_declared( + node_, name + ".subscribe_toggle", rclcpp::ParameterValue(false)); + // Optionally determine if docked via stall detection using joint_states nav2::declare_parameter_if_not_declared( node_, name + ".use_stall_detection", rclcpp::ParameterValue(false)); @@ -94,6 +105,26 @@ void SimpleNonChargingDock::configure( node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + node_->get_parameter(name + ".detector_service_name", detector_service_name_); + node_->get_parameter(name + ".detector_service_timeout", detector_service_timeout_); + node_->get_parameter(name + ".subscribe_toggle", subscribe_toggle_); + + // Initialize detection state + detection_active_ = false; + initial_pose_received_ = false; + + // Create persistent subscription if toggling is disabled. + if (use_external_detection_pose_ && !subscribe_toggle_) { + dock_pose_.header.stamp = rclcpp::Time(0); + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + std::string dock_direction; node_->get_parameter(name + ".dock_direction", dock_direction); dock_direction_ = utils::getDockDirectionFromString(dock_direction); @@ -112,13 +143,9 @@ void SimpleNonChargingDock::configure( node_->get_parameter(name + ".filter_coef", filter_coef); filter_ = std::make_unique(filter_coef, external_detection_timeout_); - if (use_external_detection_pose_) { - dock_pose_.header.stamp = rclcpp::Time(0); - dock_pose_sub_ = node_->create_subscription( - "detected_dock_pose", - [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { - detected_dock_pose_ = *pose; - }, nav2::qos::StandardTopicQoS(1)); // Only want the most recent one + if (!detector_service_name_.empty()) { + detector_client_ = node_->create_client( + detector_service_name_, false); } bool use_stall_detection; @@ -178,6 +205,12 @@ bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pos return true; } + // Guard against using pose data before the first detection has arrived. + if (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Waiting for first detected_dock_pose; none received yet"); + return false; + } + // If using detections, get current detections, transform to frame, and apply offsets geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; @@ -289,6 +322,120 @@ void SimpleNonChargingDock::jointStateCallback(const sensor_msgs::msg::JointStat is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_); } +bool SimpleNonChargingDock::startDetectionProcess() +{ + // Skip if already active + if (detection_active_) { + return true; + } + + // 1. Service START request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to start.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Subscription toggle + // Only subscribe once; will set state to ON on first message + if (subscribe_toggle_ && !dock_pose_sub_) { + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + initial_pose_received_ = true; + }, + nav2::qos::StandardTopicQoS()); + } + + detection_active_ = true; + RCLCPP_INFO(node_->get_logger(), "External detector activation requested."); + return true; +} + +bool SimpleNonChargingDock::stopDetectionProcess() +{ + // Skip if already OFF + if (!detection_active_) { + return true; + } + + // 1. Service STOP request + if (detector_client_) { + auto req = std::make_shared(); + try { + auto future = detector_client_->invoke( + req, + std::chrono::duration_cast( + std::chrono::duration(detector_service_timeout_))); + + if (!future || !future->success) { + RCLCPP_ERROR( + node_->get_logger(), "Detector service '%s' failed to stop.", + detector_service_name_.c_str()); + return false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Calling detector service '%s' failed: %s", + detector_service_name_.c_str(), e.what()); + return false; + } + } + + // 2. Unsubscribe to release resources + // reset() will tear down the topic subscription immediately + if (subscribe_toggle_ && dock_pose_sub_) { + dock_pose_sub_.reset(); + } + + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_INFO(node_->get_logger(), "External detector deactivation requested."); + return true; +} + +void SimpleNonChargingDock::activate() +{ + dock_pose_pub_->on_activate(); + filtered_dock_pose_pub_->on_activate(); + staging_pose_pub_->on_activate(); +} + +void SimpleNonChargingDock::deactivate() +{ + stopDetectionProcess(); + dock_pose_pub_->on_deactivate(); + filtered_dock_pose_pub_->on_deactivate(); + staging_pose_pub_->on_deactivate(); + RCLCPP_DEBUG(node_->get_logger(), "SimpleNonChargingDock deactivated"); +} + +void SimpleNonChargingDock::cleanup() +{ + detector_client_.reset(); + dock_pose_sub_.reset(); + detection_active_ = false; + initial_pose_received_ = false; + RCLCPP_DEBUG(node_->get_logger(), "SimpleNonChargingDock cleaned up"); +} + } // namespace opennav_docking #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index 8708b31428d..e8de87d99c6 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -291,6 +291,8 @@ TEST(ControllerTests, CollisionCheckerDockForward) { node, tf, "test_base_frame", "test_base_frame"); collision_tester->configure(); collision_tester->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // 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); @@ -302,7 +304,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { // Publish an empty costmap // It should not hit anything in an empty costmap collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); 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 @@ -310,7 +312,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { // 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()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); // Set an object between the robot and the dock @@ -318,7 +320,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); // Set the collision tolerance to 0 to ensure all obstacles in the path are detected @@ -329,7 +331,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); collision_tester->deactivate(); @@ -356,6 +358,8 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { node, tf, "test_base_frame", "test_base_frame"); collision_tester->configure(); collision_tester->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Set the pose of the dock at 1.75m behind the robot auto dock_pose = collision_tester->setPose(-1.75, 0.0, 0.0); @@ -367,7 +371,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { // Publish an empty costmap // It should not hit anything in an empty costmap collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); // Set a dock in the costmap of 0.2x1.5m at 2m behind the robot @@ -375,7 +379,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { // 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()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); // Set an object between the robot and the dock @@ -383,7 +387,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); // Set the collision tolerance to 0 to ensure all obstacles in the path are detected @@ -394,7 +398,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); collision_tester->deactivate(); @@ -421,6 +425,8 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { node, tf, "test_base_frame", "test_base_frame"); collision_tester->configure(); collision_tester->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // Set the staging pose at 1.75m behind the robot auto staging_pose = collision_tester->setPose(-1.75, 0.0, 0.0); @@ -432,7 +438,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { // Publish an empty costmap // It should not hit anything in an empty costmap collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); 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 @@ -440,7 +446,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { // 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()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); // Set an object beyond the staging pose @@ -448,7 +454,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); // Set an object between the robot and the staging pose @@ -456,7 +462,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); // Set the collision tolerance to 0 to ensure all obstacles in the path are detected @@ -467,7 +473,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); collision_tester->deactivate(); @@ -494,6 +500,8 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { node, tf, "test_base_frame", "test_base_frame"); collision_tester->configure(); collision_tester->activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // 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); @@ -505,14 +513,14 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { // Publish an empty costmap // It should not hit anything in an empty costmap collision_tester->publishCostmap(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); 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()); + executor.spin_some(); EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); // Set an object beyond the staging pose @@ -520,7 +528,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); // Set an object between the robot and the staging pose @@ -528,7 +536,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); // Set the collision tolerance to 0 to ensure all obstacles in the path are detected @@ -539,7 +547,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { 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()); + executor.spin_some(); EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); collision_tester->deactivate(); diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 8974c236ab0..46d69b0e40c 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -28,6 +28,7 @@ import launch_testing.asserts import launch_testing.markers import launch_testing.util +from lifecycle_msgs.srv import GetState from nav2_common.launch import RewrittenYaml from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot from nav_msgs.msg import Odometry @@ -36,6 +37,7 @@ from rclpy.action.client import ActionClient from rclpy.action.server import ActionServer, ServerGoalHandle from sensor_msgs.msg import BatteryState +import tf2_ros from tf2_ros import TransformBroadcaster # This test can be run standalone with: @@ -127,8 +129,28 @@ def setUp(self) -> None: self.command = Twist() self.node = rclpy.create_node('test_docking_server') # Publish odometry + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) self.odom_pub = self.node.create_publisher(Odometry, 'odom', 10) + def wait_for_node_to_be_active(self, node_name, timeout_sec=10.0): + """Wait for a managed node to become active.""" + client = self.node.create_client(GetState, f'{node_name}/get_state') + if not client.wait_for_service(timeout_sec=2.0): + self.fail(f'Service get_state for {node_name} not available.') + + start_time = time.time() + while time.time() - start_time < timeout_sec: + req = GetState.Request() + future = client.call_async(req) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=1.0) + if future.result() and future.result().current_state.id == 3: # 3 = ACTIVE + self.node.get_logger().info(f'Node {node_name} is active.') + return + time.sleep(0.5) + # raises AssertionError + self.fail(f'Node {node_name} did not become active within {timeout_sec} seconds.') + def tearDown(self) -> None: self.node.destroy_node() @@ -206,6 +228,7 @@ def nav_execute_callback( def test_docking_server(self) -> None: # Publish TF for odometry self.tf_broadcaster = TransformBroadcaster(self.node) + time.sleep(0.5) # Create a timer to run "control loop" at 20hz self.timer = self.node.create_timer(0.05, self.timer_callback) @@ -245,10 +268,19 @@ def test_docking_server(self) -> None: # Publish transform self.publish() - # Run for 1 seconds to allow tf to propagate - for _ in range(10): + # Wait until the transform is available. + self.node.get_logger().info('Waiting for TF odom->base_link to be available...') + start_time = time.time() + timeout = 10.0 + while not self.tf_buffer.can_transform('odom', 'base_link', rclpy.time.Time()): + if time.time() - start_time > timeout: + self.fail('TF transform odom->base_link not available after 10s') rclpy.spin_once(self.node, timeout_sec=0.1) time.sleep(0.1) + self.node.get_logger().info('TF is ready, proceeding with test.') + + # Wait until the docking server is active. + self.wait_for_node_to_be_active('docking_server') # Test docking action self.action_result = [] diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 7cc1f2fe8ab..22d676bf60a 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -46,6 +46,15 @@ class DockingServerShim : public DockingServer } }; +// Test shim to expose the TF2 buffer for injection +class DockingServerTFShim : public DockingServerShim +{ +public: + DockingServerTFShim() + : DockingServerShim() {} + std::shared_ptr getTfBuffer() {return tf2_buffer_;} +}; + TEST(DockingServerTests, ObjectLifecycle) { auto node = std::make_shared(); @@ -311,6 +320,161 @@ TEST(DockingServerTests, testDockBackward) node.reset(); } +TEST(DockingServerTests, ExceptionHandlingDuringDocking) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto client_node = std::make_shared("test_client"); + + // Configure docking server + node->declare_parameter("docks", std::vector{"test_dock"}); + node->declare_parameter("test_dock.type", "test_plugin"); + node->declare_parameter("test_dock.pose", std::vector{0.0, 0.0, 0.0}); + node->declare_parameter("dock_plugins", std::vector{"test_plugin"}); + node->declare_parameter("test_plugin.plugin", "opennav_docking::TestFailureDock"); + node->declare_parameter("exception_to_throw", ""); + node->declare_parameter("dock_action_called", false); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + // Test multiple exception scenarios to ensure coverage + std::vector exceptions = {"FailedToDetectDock", "FailedToControl"}; + + for (const auto & exception_type : exceptions) { + node->set_parameter(rclcpp::Parameter("exception_to_throw", exception_type)); + node->set_parameter(rclcpp::Parameter("dock_action_called", false)); + + auto client = rclcpp_action::create_client(client_node, "dock_robot"); + ASSERT_TRUE(client->wait_for_action_server(2s)); + + auto goal = DockRobot::Goal(); + goal.dock_id = "test_dock"; + goal.navigate_to_staging_pose = false; + + auto future_goal = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(client_node, future_goal, 2s); + + auto goal_handle = future_goal.get(); + ASSERT_TRUE(goal_handle); + + auto future_result = client->async_get_result(goal_handle); + auto status = rclcpp::spin_until_future_complete(client_node, future_result, 5s); + ASSERT_EQ(status, rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); + } + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); +} + +TEST(DockingServerTests, StopDetectionOnSuccess) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto client_node = std::make_shared("test_client_success"); + + // Configure the server with the test plugin + node->declare_parameter("docks", std::vector{"test_dock"}); + node->declare_parameter("test_dock.type", "test_plugin"); + node->declare_parameter("test_dock.pose", std::vector{0.0, 0.0, 0.0}); + node->declare_parameter("dock_plugins", std::vector{"test_plugin"}); + node->declare_parameter("test_plugin.plugin", "opennav_docking::TestFailureDock"); + + // Configure TestFailureDock to report success + node->declare_parameter("exception_to_throw", ""); + node->declare_parameter("dock_action_called", true); + // Note: isCharging() in TestFailureDock returns false, so it will wait for charge + // which will succeed because the plugin is a charger. We'll set the timeout low. + node->set_parameter(rclcpp::Parameter("wait_charge_timeout", 0.1)); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + auto client = rclcpp_action::create_client(client_node, "dock_robot"); + ASSERT_TRUE(client->wait_for_action_server(2s)); + + DockRobot::Goal goal; + goal.dock_id = "test_dock"; + goal.navigate_to_staging_pose = false; + + auto future_goal = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(client_node, future_goal, 2s); + auto goal_handle = future_goal.get(); + ASSERT_TRUE(goal_handle); + + auto future_result = client->async_get_result(goal_handle); + ASSERT_EQ( + rclcpp::spin_until_future_complete(client_node, future_result, 5s), + rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_TRUE(result.result->success); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->shutdown(); +} + +TEST(DockingServerTests, HandlesPluginStartFailure) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto client_node = std::make_shared("test_client_start_failure"); + + // Configure the server with the TestFailureDock plugin. + node->declare_parameter("docks", std::vector{"test_dock"}); + node->declare_parameter("test_dock.type", "test_plugin"); + node->declare_parameter("test_dock.pose", std::vector{0.0, 0.0, 0.0}); + node->declare_parameter("test_dock.frame", "odom"); + node->declare_parameter("dock_plugins", std::vector{"test_plugin"}); + node->declare_parameter("test_plugin.plugin", "opennav_docking::TestFailureDock"); + node->declare_parameter("exception_to_throw", ""); + + // Configure the TestFailureDock to fail its startup process. + node->declare_parameter("fail_start_detection", true); + node->declare_parameter("dock_action_called", false); + + node->on_configure(rclcpp_lifecycle::State()); + + // Mock the necessary TF transform to prevent a premature failure. + geometry_msgs::msg::TransformStamped identity_transform; + identity_transform.header.frame_id = "odom"; + identity_transform.child_frame_id = "odom"; + identity_transform.transform.rotation.w = 1.0; + node->getTfBuffer()->setTransform(identity_transform, "test_authority", true); + + node->on_activate(rclcpp_lifecycle::State()); + + auto client = rclcpp_action::create_client(client_node, "dock_robot"); + ASSERT_TRUE(client->wait_for_action_server(2s)); + + DockRobot::Goal goal; + goal.dock_id = "test_dock"; + goal.navigate_to_staging_pose = false; + + auto future_goal = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(client_node, future_goal, 2s); + auto goal_handle = future_goal.get(); + ASSERT_TRUE(goal_handle); + + auto future_result = client->async_get_result(goal_handle); + ASSERT_EQ( + rclcpp::spin_until_future_complete(client_node, future_result, 5s), + rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); + EXPECT_EQ(result.result->error_code, DockRobot::Result::FAILED_TO_DETECT_DOCK); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->shutdown(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index 293cfcc8933..a95bd36eda3 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "gtest/gtest.h" +#include "std_srvs/srv/trigger.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -24,6 +26,8 @@ // Testing the simple charging dock plugin +using namespace std::chrono_literals; + namespace opennav_docking { @@ -39,6 +43,15 @@ class SimpleChargingDockShim : public opennav_docking::SimpleChargingDock } }; +class SimpleChargingDockTestable : public opennav_docking::SimpleChargingDock +{ +public: + using opennav_docking::SimpleChargingDock::SimpleChargingDock; + + // Expose detector state for test verification + bool isDetectorActive() const {return initial_pose_received_;} +}; + TEST(SimpleChargingDockTests, ObjectLifecycle) { auto node = std::make_shared("test"); @@ -47,6 +60,8 @@ TEST(SimpleChargingDockTests, ObjectLifecycle) auto dock = std::make_unique(); dock->configure(node, "my_dock", nullptr); dock->activate(); + EXPECT_TRUE(dock->startDetectionProcess()); + EXPECT_TRUE(dock->stopDetectionProcess()); // Check initial states EXPECT_FALSE(dock->isCharging()); @@ -229,6 +244,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); + dock->startDetectionProcess(); geometry_msgs::msg::PoseStamped pose; @@ -243,6 +259,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); pose.header.frame_id = "my_frame"; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -250,6 +267,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -306,14 +324,7 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - - geometry_msgs::msg::PoseStamped detected_pose; - detected_pose.header.stamp = node->now(); - detected_pose.header.frame_id = "my_frame"; - detected_pose.pose.position.x = 1.0; - detected_pose.pose.position.y = 1.0; - pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + dock->startDetectionProcess(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -326,11 +337,24 @@ TEST(SimpleChargingDockTests, IsDockedTransformException) transform.child_frame_id = "other_frame"; tf_buffer->setTransform(transform, "test", true); - // It can find a transform between the two frames but it throws an exception in isDocked + // First call to getRefinedPose starts detection + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + // Now publish the detection after subscription is created + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Second call should succeed EXPECT_TRUE(dock->getRefinedPose(pose, "")); EXPECT_FALSE(dock->isDocked()); dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -401,6 +425,208 @@ TEST(SimpleChargingDockTests, ShouldRotateToDock) dock.reset(); } +TEST(SimpleChargingDockTests, DetectorLifecycle) +{ + auto node = std::make_shared("test"); + + // Test with detector service configured + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + node->declare_parameter("my_dock.detector_service_name", + rclcpp::ParameterValue("test_detector_service")); + node->declare_parameter("my_dock.subscribe_toggle", rclcpp::ParameterValue(true)); + + // Create a mock service to prevent timeout + bool service_called = false; + auto service = node->create_service( + "test_detector_service", + [&service_called](std::shared_ptr, + std::shared_ptr, + std::shared_ptr response) + { + service_called = true; + response->success = true; + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + + dock->activate(); + dock->startDetectionProcess(); + // Spin to process async service call + auto start_time = std::chrono::steady_clock::now(); + while (!service_called && + std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2)) + { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + EXPECT_TRUE(service_called); + dock->stopDetectionProcess(); + dock->deactivate(); + + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleChargingDockTests, DetectorServiceConfiguration) +{ + auto node = std::make_shared("test_detector_config"); + + // Configure with detector service + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "detector_service"); + node->declare_parameter("my_dock.detector_service_timeout", 1.0); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_NO_THROW(dock->activate()); + + EXPECT_NO_THROW(dock->deactivate()); + EXPECT_NO_THROW(dock->cleanup()); +} + +TEST(SimpleChargingDockTests, SubscriptionCallback) +{ + auto node = std::make_shared("test_subscription_reliable"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.subscribe_toggle", true); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + // A LifecyclePublisher must be activated to publish. + publisher->on_activate(); + + dock->startDetectionProcess(); + + // Wait for the publisher and subscriber to connect. + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + // Publish a message to trigger the subscription callback. + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state was updated, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleChargingDockTests, DetectorServiceTimeout) +{ + auto node = std::make_shared("test_detector_timeout"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "slow_service"); + node->declare_parameter("my_dock.detector_service_timeout", 0.1); + + // Create a mock service that never responds in time + auto mock_service = node->create_service( + "slow_service", + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The call to invoke() should timeout and be caught + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); + node->shutdown(); +} + +TEST(SimpleChargingDockTests, DetectorServiceFailure) +{ + const char * service_name = "charging_dock_slow_service"; + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.detector_service_name", std::string(service_name)}, + // The client will time out after 100ms + {"my_dock.detector_service_timeout", 0.1} + }); + auto node = std::make_shared("test_detector_failure", options); + + // Create a service that responds slower than the client's timeout. + auto slow_service = node->create_service( + service_name, + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The invoke() call should time out, but the exception must be caught + // within the startDetectionProcess method. The test passes if no + // uncaught exception is thrown. + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleChargingDockTests, SubscriptionPersistent) +{ + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.subscribe_toggle", false} // The key parameter to test. + }); + auto node = std::make_shared("test_sub_persistent", options); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The subscription should be active immediately after configuration. + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + publisher->on_activate(); + + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state changed, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp index 61d3207e150..77951bd6cd5 100644 --- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "gtest/gtest.h" +#include "std_srvs/srv/trigger.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -24,6 +26,8 @@ // Testing the simple non-charging dock plugin +using namespace std::chrono_literals; + namespace opennav_docking { @@ -39,6 +43,15 @@ class SimpleNonChargingDockShim : public opennav_docking::SimpleNonChargingDock } }; +class SimpleNonChargingDockTestable : public opennav_docking::SimpleNonChargingDock +{ +public: + using opennav_docking::SimpleNonChargingDock::SimpleNonChargingDock; + + // Expose detector state for test verification + bool isDetectorActive() const {return initial_pose_received_;} +}; + TEST(SimpleNonChargingDockTests, ObjectLifecycle) { auto node = std::make_shared("test"); @@ -47,6 +60,8 @@ TEST(SimpleNonChargingDockTests, ObjectLifecycle) auto dock = std::make_unique(); dock->configure(node, "my_dock", nullptr); dock->activate(); + EXPECT_TRUE(dock->startDetectionProcess()); + EXPECT_TRUE(dock->stopDetectionProcess()); // Check initial states EXPECT_THROW(dock->isCharging(), std::runtime_error); @@ -190,6 +205,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) dock->configure(node, "my_dock", nullptr); dock->activate(); + dock->startDetectionProcess(); geometry_msgs::msg::PoseStamped pose; @@ -204,6 +220,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) detected_pose.pose.position.y = -0.5; pub->publish(detected_pose); rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); pose.header.frame_id = "my_frame"; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -211,6 +228,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -267,14 +285,7 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) dock->configure(node, "my_dock", tf_buffer); dock->activate(); - - geometry_msgs::msg::PoseStamped detected_pose; - detected_pose.header.stamp = node->now(); - detected_pose.header.frame_id = "my_frame"; - detected_pose.pose.position.x = 1.0; - detected_pose.pose.position.y = 1.0; - pub->publish(detected_pose); - rclcpp::spin_some(node->get_node_base_interface()); + dock->startDetectionProcess(); // Create a pose with a different frame_id geometry_msgs::msg::PoseStamped pose; @@ -287,11 +298,24 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException) transform.child_frame_id = "other_frame"; tf_buffer->setTransform(transform, "test", true); - // It can find a transform between the two frames but it throws an exception in isDocked + // First call to getRefinedPose starts detection + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + // Now publish the detection after subscription is created + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Second call should succeed EXPECT_TRUE(dock->getRefinedPose(pose, "")); EXPECT_FALSE(dock->isDocked()); dock->deactivate(); + dock->stopDetectionProcess(); dock->cleanup(); dock.reset(); } @@ -362,6 +386,211 @@ TEST(SimpleChargingDockTests, ShouldRotateToDock) dock.reset(); } +TEST(SimpleNonChargingDockTests, DetectorLifecycle) +{ + auto node = std::make_shared("test"); + + // Test with detector service configured + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + node->declare_parameter("my_dock.detector_service_name", + rclcpp::ParameterValue("test_detector_service")); + node->declare_parameter("my_dock.subscribe_toggle", rclcpp::ParameterValue(true)); + + // Create a mock service to prevent timeout + bool service_called = false; + auto service = node->create_service( + "test_detector_service", + [&service_called](std::shared_ptr, + std::shared_ptr, + std::shared_ptr response) + { + service_called = true; + response->success = true; + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + + dock->activate(); + dock->startDetectionProcess(); + // Spin to process async service call + auto start_time = std::chrono::steady_clock::now(); + while (!service_called && + std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2)) + { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + EXPECT_TRUE(service_called); + dock->stopDetectionProcess(); + dock->deactivate(); + + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, DetectorServiceConfiguration) +{ + auto node = std::make_shared("test_detector_config"); + + // Configure with detector service + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "detector_service"); + node->declare_parameter("my_dock.detector_service_timeout", 1.0); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_NO_THROW(dock->activate()); + + EXPECT_NO_THROW(dock->deactivate()); + EXPECT_NO_THROW(dock->cleanup()); +} + +TEST(SimpleNonChargingDockTests, SubscriptionCallback) +{ + auto node = std::make_shared( + "test_subscription_reliable_non_charging"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.subscribe_toggle", true); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + // A LifecyclePublisher must be activated to publish. + publisher->on_activate(); + + dock->startDetectionProcess(); + + // Wait for the publisher and subscriber to connect. + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + // Publish a message to trigger the subscription callback. + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state was updated, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleNonChargingDockTests, DetectorServiceTimeout) +{ + auto node = std::make_shared("test_detector_timeout"); + + node->declare_parameter("my_dock.use_external_detection_pose", true); + node->declare_parameter("my_dock.detector_service_name", "slow_service"); + node->declare_parameter("my_dock.detector_service_timeout", 0.1); + + // Create a mock service that never responds in time + auto mock_service = node->create_service( + "slow_service", + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The call to invoke() should timeout and be caught + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); + node->shutdown(); +} + +TEST(SimpleNonChargingDockTests, DetectorServiceFailure) +{ + const char * service_name = "non_charging_dock_slow_service"; + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.detector_service_name", std::string(service_name)}, + // The client will time out after 100ms + {"my_dock.detector_service_timeout", 0.1} + }); + auto node = std::make_shared( + "test_detector_failure_non_charging", options); + + // Create a service that responds slower than the client's timeout. + auto slow_service = node->create_service( + service_name, + [](std::shared_ptr, + std::shared_ptr, + std::shared_ptr) + { + std::this_thread::sleep_for(200ms); + }); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The invoke() call should time out, but the exception must be caught + // within the startDetectionProcess method. The test passes if no + // uncaught exception is thrown. + EXPECT_NO_THROW(dock->startDetectionProcess()); + + dock->deactivate(); + dock->cleanup(); +} + +TEST(SimpleNonChargingDockTests, SubscriptionPersistent) +{ + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.use_external_detection_pose", true}, + {"my_dock.subscribe_toggle", false} // The key parameter to test. + }); + auto node = std::make_shared( + "test_sub_persistent_non_charging", options); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // The subscription should be active immediately after configuration. + auto publisher = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + publisher->on_activate(); + + int tries = 0; + while (publisher->get_subscription_count() == 0 && tries++ < 10) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } + ASSERT_GT(publisher->get_subscription_count(), 0); + + publisher->publish(geometry_msgs::msg::PoseStamped{}); + std::this_thread::sleep_for(50ms); + rclcpp::spin_some(node->get_node_base_interface()); + + // Verify the detector state changed, proving the callback was executed. + EXPECT_TRUE(dock->isDetectorActive()); + + dock->deactivate(); + dock->cleanup(); +} + } // namespace opennav_docking int main(int argc, char **argv) diff --git a/nav2_docking/opennav_docking/test/testing_dock.cpp b/nav2_docking/opennav_docking/test/testing_dock.cpp index 8a33c3b3304..a5ee4adaff5 100644 --- a/nav2_docking/opennav_docking/test/testing_dock.cpp +++ b/nav2_docking/opennav_docking/test/testing_dock.cpp @@ -102,6 +102,18 @@ class TestFailureDock : public opennav_docking_core::ChargingDock return true; } + virtual bool startDetectionProcess() + { + bool should_fail; + node_->get_parameter_or("fail_start_detection", should_fail, false); + return !should_fail; + } + + virtual bool stopDetectionProcess() + { + return true; + } + protected: nav2::LifecycleNode::SharedPtr node_; }; diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index 67e599405d7..111dc1c03ef 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -124,6 +124,16 @@ class ChargingDock */ virtual bool hasStoppedCharging() = 0; + /** + * @brief Start any detection pipelines required for pose refinement. + */ + virtual bool startDetectionProcess() = 0; + + /** + * @brief Stop any detection pipelines running for pose refinement. + */ + virtual bool stopDetectionProcess() = 0; + /** * @brief Gets if this is a charging-typed dock */ diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp index 9422ee19f13..97b9dd06e61 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp @@ -125,6 +125,16 @@ class NonChargingDock : public ChargingDock throw std::runtime_error("This dock is not a charging dock!"); } + /** + * @brief Start any detection pipelines required for pose refinement. + */ + virtual bool startDetectionProcess() = 0; + + /** + * @brief Stop any detection pipelines running for pose refinement. + */ + virtual bool stopDetectionProcess() = 0; + /** * @brief Gets if this is a charging-typed dock */ diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index a003a8fb087..6459b42ed2c 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -5,21 +5,26 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) find_package(backward_ros REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(GRAPHICSMAGICKCPP REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(PkgConfig REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) find_package(nav2_ros_common REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaml-cpp REQUIRED) +pkg_search_module(UUID REQUIRED uuid) + nav2_package() set(map_server_executable map_server) @@ -28,12 +33,16 @@ set(library_name ${map_server_executable}_core) set(map_io_library_name map_io) +set(vo_library_name vector_object_core) + set(map_saver_cli_executable map_saver_cli) set(map_saver_server_executable map_saver_server) set(costmap_filter_info_server_executable costmap_filter_info_server) +set(vo_server_executable vector_object_server) + add_library(${map_io_library_name} SHARED src/map_mode.cpp src/map_io.cpp) @@ -78,6 +87,31 @@ target_link_libraries(${library_name} PRIVATE yaml-cpp::yaml-cpp ) +add_library(${vo_library_name} SHARED + src/vo_server/vector_object_shapes.cpp + src/vo_server/vector_object_server.cpp) +target_include_directories(${vo_library_name} + PUBLIC + "$" + "$" + "$") +target_link_libraries(${vo_library_name} PUBLIC + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + rclcpp_components::component + tf2_ros::tf2_ros + ${UUID_LIBRARIES} +) +target_link_libraries(${library_name} PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + yaml-cpp::yaml-cpp +) + add_executable(${map_server_executable} src/map_server/main.cpp) target_include_directories(${map_server_executable} @@ -142,12 +176,28 @@ target_link_libraries(${costmap_filter_info_server_executable} PRIVATE rclcpp_lifecycle::rclcpp_lifecycle ) +add_executable(${vo_server_executable} + src/vo_server/main.cpp) +target_include_directories(${vo_server_executable} + PRIVATE + "$" + "$") +target_link_libraries(${vo_server_executable} PRIVATE + ${vo_library_name} + ${nav_msgs_TARGETS} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) + rclcpp_components_register_nodes(${library_name} "nav2_map_server::CostmapFilterInfoServer") rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapSaver") rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapServer") +rclcpp_components_register_nodes(${vo_library_name} "nav2_map_server::VectorObjectServer") install(TARGETS - ${library_name} ${map_io_library_name} + ${library_name} ${map_io_library_name} ${vo_library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -155,13 +205,14 @@ install(TARGETS install(TARGETS ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} - ${costmap_filter_info_server_executable} + ${costmap_filter_info_server_executable} ${vo_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -178,6 +229,7 @@ ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries( ${library_name} ${map_io_library_name} + ${vo_library_name} ) ament_export_dependencies(nav_msgs nav2_msgs nav2_util rclcpp rclcpp_lifecycle nav2_ros_common) ament_export_targets(${library_name}) diff --git a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp new file mode 100644 index 00000000000..ed4186722bc --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -0,0 +1,228 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" + +#include "nav2_msgs/srv/add_shapes.hpp" +#include "nav2_msgs/srv/remove_shapes.hpp" +#include "nav2_msgs/srv/get_shapes.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +#include "nav2_map_server/vector_object_utils.hpp" +#include "nav2_map_server/vector_object_shapes.hpp" + +namespace nav2_map_server +{ + +/// @brief Vector Object server node +class VectorObjectServer : public nav2::LifecycleNode +{ +public: + /** + * @brief Constructor for the VectorObjectServer + * @param options Additional options to control creation of the node. + */ + explicit VectorObjectServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +protected: + /** + * @brief: Initializes TF buffer/listener, obtains ROS-parameters, creates incoming services, + * and output map publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Activates output map publisher and creates bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Deactivates map publisher and timer (if any), destroys bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Resets all services, publishers, map and TF-s + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called in shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Supporting routine obtaining all ROS-parameters + * @return True if all parameters were obtained or false in the failure case + */ + bool obtainParams(); + + /** + * @brief Finds the shape with given UUID + * @param uuid Given UUID to search with + * @return Iterator to the shape, if found. Otherwise past-the-end iterator. + */ + std::vector>::iterator findShape(const unsigned char * uuid); + + /** + * @brief Transform all vector shapes from their local frame to output map frame + * @return Whether all vector objects were transformed successfully + */ + bool transformVectorObjects(); + + /** + * @brief Obtains map boundaries to place all vector objects inside + * @param min_x output min X-boundary of required map + * @param min_y output min Y-boundary of required map + * @param max_x output max X-boundary of required map + * @param max_y output max Y-boundary of required map + * @throw std::exception if can not obtain one of the map boundaries + */ + void getMapBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) const; + + /** + * @brief Creates or updates existing map with required sizes and fills it with default value + * @param min_x min X-boundary of new map + * @param min_y min Y-boundary of new map + * @param max_x max X-boundary of new map + * @param max_y max Y-boundary of new map + * @throw std::exception if map has negative X or Y size + */ + void updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y); + + /** + * @brief Processes all vector objects on raster output map + */ + void putVectorObjectsOnMap(); + + /** + * @brief Publishes output map + */ + void publishMap(); + + /** + * @brief Calculates new map sizes, updates map, processes all vector objects on it + * and publishes output map one time + */ + void processMap(); + + /** + * @brief If map to be update dynamically, creates map processing timer, + * otherwise process map once + */ + void switchMapUpdate(); + + /** + * @brief Callback for AddShapes service call. + * Reads all input vector objects from service request, + * creates them or updates their shape in case of existing objects + * and switches map processing/publishing routine + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void addShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Callback for GetShapes service call. + * Gets all shapes and returns them to the service response + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void getShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Callback for RemoveShapes service call. + * Try to remove requested vector objects and switches map processing/publishing routine + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void removeShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + +protected: + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief TF listener + std::shared_ptr tf_listener_; + + /// @brief All shapes vector + std::vector> shapes_; + + /// @brief Output map resolution + double resolution_; + /// @brief Default value the output map to be filled with + int8_t default_value_; + /// @brief @Overlay Type of overlay of vector objects on the map + OverlayType overlay_type_; + + /// @brief Output map with vector objects on it + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + /// @brief Whether to process and publish map + double process_map_; + + /// @brief Frame of output map + std::string global_frame_id_; + /// @brief Transform tolerance + double transform_tolerance_; + + /// @brief Frequency to dynamically update/publish the map (if necessary) + double update_frequency_; + /// @brief Map update timer + rclcpp::TimerBase::SharedPtr map_timer_; + + /// @brief AddShapes service + nav2::ServiceServer::SharedPtr add_shapes_service_; + /// @brief GetShapes service + nav2::ServiceServer::SharedPtr get_shapes_service_; + /// @brief RemoveShapes service + nav2::ServiceServer::SharedPtr remove_shapes_service_; + + /// @beirf Topic name where the output map to be published to + std::string map_topic_; + /// @brief Output map publisher + nav2::Publisher::SharedPtr map_pub_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp new file mode 100644 index 00000000000..dd5cae95b62 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -0,0 +1,413 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/polygon.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.hpp" + +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +#include "nav2_map_server/vector_object_utils.hpp" + +namespace nav2_map_server +{ + +/// @brief Possible vector object shape types +enum ShapeType +{ + UNKNOWN = 0, + POLYGON = 1, + CIRCLE = 2 +}; + +/// @brief Basic class, other vector objects to be inherited from +class Shape +{ +public: + /** + * @brief Shape basic class constructor + * @param node Vector Object server node pointer + */ + explicit Shape(const nav2::LifecycleNode::WeakPtr & node); + + /** + * @brief Shape destructor + */ + virtual ~Shape(); + + /** + * @brief Returns type of the shape + * @return Type of the shape + */ + ShapeType getType(); + + /** + * @brief Supporting routine obtaining shape UUID from ROS-parameters + * for the given shape object + * @return True if UUID was obtained or false in failure case + */ + bool obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid); + + /** + * @brief Gets the value of the shape. + * Empty virtual method intended to be used in child implementations + * @return OccupancyGrid value of the shape + */ + virtual int8_t getValue() const = 0; + + /** + * @brief Gets frame ID of the shape. + * Empty virtual method intended to be used in child implementations + * @return Frame ID where the shape is + */ + virtual std::string getFrameID() const = 0; + + /** + * @brief Gets UUID of the shape. + * Empty virtual method intended to be used in child implementations + * @return Shape UUID string + */ + virtual std::string getUUID() const = 0; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * Empty virtual method intended to be used in child implementations + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + virtual bool isUUID(const unsigned char * uuid) const = 0; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * Empty virtual method intended to be used in child implementations + * @return True if shape to be filled + */ + virtual bool isFill() const = 0; + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * Empty virtual method intended to be used in child implementations + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + virtual bool obtainParams(const std::string & shape_name) = 0; + + /** + * @brief Transforms shape coordinates to a new frame. + * Empty virtual method intended to be used in child implementations + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + virtual bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) = 0; + + /** + * @brief Gets shape box-boundaries. + * Empty virtual method intended to be used in child implementations + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + virtual void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) = 0; + + /** + * @brief Is the point inside the shape. + * Empty virtual method intended to be used in child implementations + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + virtual bool isPointInside(const double px, const double py) const = 0; + + /** + * @brief Puts shape borders on map. + * Empty virtual method intended to be used in child implementations + * @param map Output map pointer + * @param overlay_type Overlay type + */ + virtual void putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0; + +protected: + /// @brief Type of shape + ShapeType type_; + + /// @brief VectorObjectServer node + nav2::LifecycleNode::WeakPtr node_; +}; + +/// @brief Polygon shape class +class Polygon : public Shape +{ +public: + /* + * @brief Polygon class constructor + * @param node Vector Object server node pointer + * @note setParams()/obtainParams() should be called after to configure the shape + */ + explicit Polygon(const nav2::LifecycleNode::WeakPtr & node); + + /** + * @brief Gets the value of the shape. + * @return OccupancyGrid value of the shape + */ + int8_t getValue() const; + + /** + * @brief Gets frame ID of the shape. + * @return Frame ID where the shape is + */ + std::string getFrameID() const; + + /** + * @brief Gets UUID of the shape. + * @return Shape UUID string + */ + std::string getUUID() const; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + bool isUUID(const unsigned char * uuid) const; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * @return True if shape to be filled + */ + bool isFill() const; + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool obtainParams(const std::string & shape_name); + + /** + * @brief Gets Polygon parameters + * @return Polygon parameters + */ + nav2_msgs::msg::PolygonObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Polygon parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); + + /** + * @brief Transforms shape coordinates to a new frame. + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance); + + /** + * @brief Gets shape box-boundaries + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Is the point inside the shape. + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + bool isPointInside(const double px, const double py) const; + + /** + * @brief Puts shape borders on map. + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + +protected: + /** + * @brief Checks that shape is consistent for further operation + * @return False in case of inconsistent shape + */ + bool checkConsistency(); + + /// @brief Input polygon parameters (could be in any frame) + nav2_msgs::msg::PolygonObject::SharedPtr params_; + /// @brief Polygon in the map's frame + geometry_msgs::msg::Polygon::SharedPtr polygon_; +}; + +/// @brief Circle shape class +class Circle : public Shape +{ +public: + /* + * @brief Circle class constructor + * @param node Vector Object server node pointer + * @note setParams()/obtainParams() should be called after to configure the shape + */ + explicit Circle(const nav2::LifecycleNode::WeakPtr & node); + + /** + * @brief Gets the value of the shape. + * @return OccupancyGrid value of the shape + */ + int8_t getValue() const; + + /** + * @brief Gets frame ID of the shape. + * @return Frame ID where the shape is + */ + std::string getFrameID() const; + + /** + * @brief Gets UUID of the shape. + * @return Shape UUID string + */ + std::string getUUID() const; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + bool isUUID(const unsigned char * uuid) const; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * @return True if shape to be filled + */ + bool isFill() const; + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool obtainParams(const std::string & shape_name); + + /** + * @brief Gets Circle parameters + * @return Circle parameters + */ + nav2_msgs::msg::CircleObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Circle parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); + + /** + * @brief Transforms shape coordinates to a new frame. + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance); + + /** + * @brief Gets shape box-boundaries + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Is the point inside the shape. + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + bool isPointInside(const double px, const double py) const; + + /** + * @brief Puts shape borders on map. + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + +protected: + /** + * @brief Checks that shape is consistent for further operation + * @return False in case of inconsistent shape + */ + bool checkConsistency(); + + /** + * @brief Converts circle center to map coordinates + * considering FP-accuracy losing on small values when using conventional + * nav2_util::worldToMap() approach + * @param map Map pointer + * @param mcx Output X-coordinate of associated circle center on map + * @param mcy Output Y-coordinate of associated circle center on map + * @return True if the conversion was successful + */ + bool centerToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + unsigned int & mcx, unsigned int & mcy); + + /** + * @brief Put Circle's point on map + * @param mx X-coordinate of given point + * @param my Y-coordinate of given point + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putPoint( + unsigned int mx, unsigned int my, + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const OverlayType overlay_type); + + /// @brief Input circle parameters (could be in any frame) + nav2_msgs::msg::CircleObject::SharedPtr params_; + /// @brief Circle center in the map's frame + geometry_msgs::msg::Point32::SharedPtr center_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp new file mode 100644 index 00000000000..f478f71b3ea --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -0,0 +1,142 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" + +namespace nav2_map_server +{ + +// ---------- Working with UUID-s ---------- + +/** + * @brief Converts UUID from input array to unparsed string + * @param uuid Input UUID in array format + * @return Unparsed UUID string + */ +inline std::string unparseUUID(const unsigned char * uuid) +{ + char uuid_str[37]; + uuid_unparse(uuid, uuid_str); + return std::string(uuid_str); +} + +// ---------- Working with shapes' overlays ---------- + +/// @brief Type of overlay between different vector objects and map +enum class OverlayType : uint8_t +{ + OVERLAY_SEQ = 0, // Vector objects are superimposed in the order in which they have arrived + OVERLAY_MAX = 1, // Maximum value from vector objects and map is being chosen + OVERLAY_MIN = 2 // Minimum value from vector objects and map is being chosen +}; + +/** + * @brief Updates map value with shape's one according to the given overlay type + * @param map_val Map value. To be updated with new value if overlay is involved + * @param shape_val Vector object value to be overlaid on map + * @param overlay_type Type of overlay + * @throw std::exception in case of unknown overlay type + */ +inline void processVal( + int8_t & map_val, const int8_t shape_val, + const OverlayType overlay_type) +{ + switch (overlay_type) { + case OverlayType::OVERLAY_SEQ: + map_val = shape_val; + return; + case OverlayType::OVERLAY_MAX: + if (shape_val > map_val) { + map_val = shape_val; + } + return; + case OverlayType::OVERLAY_MIN: + if ((map_val == nav2_util::OCC_GRID_UNKNOWN || shape_val < map_val) && + shape_val != nav2_util::OCC_GRID_UNKNOWN) + { + map_val = shape_val; + } + return; + default: + throw std::runtime_error{"Unknown overlay type"}; + } +} + +/** + * @brief Updates the cell on the map with given shape value according to the given overlay type + * @param map Output map to be updated with + * @param offset Offset to the cell to be updated + * @param shape_val Vector object value to be updated map with + * @param overlay_type Type of overlay + */ +inline void processCell( + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const unsigned int offset, + const int8_t shape_val, + const OverlayType overlay_type) +{ + int8_t map_val = map->data[offset]; + processVal(map_val, shape_val, overlay_type); + map->data[offset] = map_val; +} + +/// @brief Functor class used in raytraceLine algorithm +class MapAction +{ +public: + /** + * @brief MapAction constructor + * @param map Pointer to output map + * @param value Value to put on map + * @param overlay_type Overlay type + */ + MapAction( + nav_msgs::msg::OccupancyGrid::SharedPtr map, + int8_t value, OverlayType overlay_type) + : map_(map), value_(value), overlay_type_(overlay_type) + {} + + /** + * @brief Map' cell updating operator + * @param offset Offset on the map where the cell to be changed + */ + inline void operator()(unsigned int offset) + { + processCell(map_, offset, value_, overlay_type_); + } + +protected: + /// @brief Output map pointer + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + /// @brief Value to put on map + int8_t value_; + /// @brief Overlay type + OverlayType overlay_type_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ diff --git a/nav2_map_server/launch/vector_object_server.launch.py b/nav2_map_server/launch/vector_object_server.launch.py new file mode 100644 index 00000000000..f3c2a20ff1b --- /dev/null +++ b/nav2_map_server/launch/vector_object_server.launch.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023 Samsung R&D Institute Russia +# +# 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 ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, PushROSNamespace, SetParameter +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description() -> LaunchDescription: + # Environment + package_dir = get_package_share_directory('nav2_map_server') + + # Constant parameters + lifecycle_nodes = ['vector_object_server', 'costmap_filter_info_server'] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Launch arguments + # 1. Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + + # 2. Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(package_dir, 'params', 'vector_object_server_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='True', + description='Automatically startup Vector Object server') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of container that nodes will load in if use composition') + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + # Declare node launching commands + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + Node( + package='nav2_map_server', + executable='vector_object_server', + name='vector_object_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_vo_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings) + ] + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::VectorObjectServer', + name='vector_object_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::CostmapFilterInfoServer', + name='costmap_filter_info_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_vo_server', + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings) + ], + ) + ] + ) + + ld = LaunchDescription() + + # Launch arguments + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + + # Node launching commands + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index ffb3d3d1fce..44045314fa3 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -22,12 +22,15 @@ yaml_cpp_vendor launch_ros launch_testing + geometry_msgs tf2 + tf2_ros nav2_msgs nav2_util graphicsmagick lifecycle_msgs nav2_ros_common + uuid ament_lint_common ament_lint_auto diff --git a/nav2_map_server/params/vector_object_server_params.yaml b/nav2_map_server/params/vector_object_server_params.yaml new file mode 100644 index 00000000000..6a6a5fc2b68 --- /dev/null +++ b/nav2_map_server/params/vector_object_server_params.yaml @@ -0,0 +1,30 @@ +vector_object_server: + ros__parameters: + map_topic: "vo_map" + global_frame_id: "map" + resolution: 0.05 + default_value: -1 + overlay_type: 0 + update_frequency: 1.0 + transform_tolerance: 0.1 + shapes: ["Poly", "Circle"] + Poly: + type: "polygon" + frame_id: "map" + closed: True + value: 100 + points: [0.3, 0.5, -0.4, 1.2, -0.4, -0.2] + Circle: + type: "circle" + frame_id: "map" + fill: True + value: 100 + center: [1.5, 0.5] + radius: 0.7 +costmap_filter_info_server: + ros__parameters: + type: 0 + filter_info_topic: "vo_costmap_filter_info" + mask_topic: "vo_map" + base: 0.0 + multiplier: 1.0 diff --git a/nav2_map_server/src/vo_server/main.cpp b/nav2_map_server/src/vo_server/main.cpp new file mode 100644 index 00000000000..d2352b7aad3 --- /dev/null +++ b/nav2_map_server/src/vo_server/main.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 "nav2_map_server/vector_object_server.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp new file mode 100644 index 00000000000..89d7c3c825f --- /dev/null +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -0,0 +1,558 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 "nav2_map_server/vector_object_server.hpp" + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/create_timer.hpp" + +#include "tf2_ros/create_timer_ros.hpp" + +#include "nav2_util/occ_grid_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" + +using namespace std::placeholders; + +namespace nav2_map_server +{ + +VectorObjectServer::VectorObjectServer(const rclcpp::NodeOptions & options) +: nav2::LifecycleNode("vector_object_server", "", options), process_map_(false) +{} + +nav2::CallbackReturn +VectorObjectServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Obtaining ROS parameters + if (!obtainParams()) { + return nav2::CallbackReturn::FAILURE; + } + + map_pub_ = create_publisher( + map_topic_, + nav2::qos::LatchedPublisherQoS()); + + add_shapes_service_ = create_service( + "~/add_shapes", + std::bind(&VectorObjectServer::addShapesCallback, this, _1, _2, _3)); + + get_shapes_service_ = create_service( + "~/get_shapes", + std::bind(&VectorObjectServer::getShapesCallback, this, _1, _2, _3)); + + remove_shapes_service_ = create_service( + "~/remove_shapes", + std::bind(&VectorObjectServer::removeShapesCallback, this, _1, _2, _3)); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + map_pub_->on_activate(); + + // Trigger map to be published + process_map_ = true; + switchMapUpdate(); + + // Creating bond connection + createBond(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + if (map_timer_) { + map_timer_->cancel(); + map_timer_.reset(); + } + process_map_ = false; + + map_pub_->on_deactivate(); + + // Destroying bond connection + destroyBond(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + add_shapes_service_.reset(); + get_shapes_service_.reset(); + remove_shapes_service_.reset(); + + map_pub_.reset(); + map_.reset(); + + shapes_.clear(); + + tf_listener_.reset(); + tf_buffer_.reset(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2::CallbackReturn::SUCCESS; +} + +bool VectorObjectServer::obtainParams() +{ + auto node = shared_from_this(); + + // Main ROS-parameters + map_topic_ = nav2::declare_or_get_parameter(node, "map_topic", std::string{"vo_map"}); + global_frame_id_ = nav2::declare_or_get_parameter(node, "global_frame_id", std::string{"map"}); + resolution_ = nav2::declare_or_get_parameter(node, "resolution", 0.05); + default_value_ = nav2::declare_or_get_parameter(node, "default_value", + static_cast(nav2_util::OCC_GRID_UNKNOWN)); + overlay_type_ = static_cast(nav2::declare_or_get_parameter(node, "overlay_type", + static_cast(OverlayType::OVERLAY_SEQ))); + update_frequency_ = nav2::declare_or_get_parameter(node, "update_frequency", 1.0); + transform_tolerance_ = nav2::declare_or_get_parameter(node, "transform_tolerance", 0.1); + + // Shapes + auto shape_names = nav2::declare_or_get_parameter(node, "shapes", std::vector()); + for (std::string shape_name : shape_names) { + std::string shape_type; + try { + shape_type = nav2::declare_or_get_parameter(node, shape_name + ".type", + rclcpp::PARAMETER_STRING); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + get_logger(), "Error while getting shape %s type: %s", shape_name.c_str(), ex.what()); + return false; + } + + if (shape_type == "polygon") { + auto polygon = std::make_shared(node); + if (!polygon->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(polygon); + } else if (shape_type == "circle") { + auto circle = std::make_shared(node); + if (!circle->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(circle); + } else { + RCLCPP_ERROR(get_logger(), + "Please specify the correct type for shape %s. Supported types are 'polygon' and 'circle'", + shape_name.c_str()); + return false; + } + } + + return true; +} + +std::vector>::iterator +VectorObjectServer::findShape(const unsigned char * uuid) +{ + for (auto it = shapes_.begin(); it != shapes_.end(); it++) { + if ((*it)->isUUID(uuid)) { + return it; + } + } + return shapes_.end(); +} + +bool VectorObjectServer::transformVectorObjects() +{ + for (auto shape : shapes_) { + if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) { + // Shape to be updated dynamically + if (!shape->toFrame(global_frame_id_, tf_buffer_, transform_tolerance_)) { + RCLCPP_ERROR( + get_logger(), "Can not transform vector object from %s to %s frame", + shape->getFrameID().c_str(), global_frame_id_.c_str()); + return false; + } + } + } + + return true; +} + +void VectorObjectServer::getMapBoundaries( + double & min_x, double & min_y, double & max_x, double & max_y) const +{ + min_x = std::numeric_limits::max(); + min_y = std::numeric_limits::max(); + max_x = std::numeric_limits::lowest(); + max_y = std::numeric_limits::lowest(); + + double min_p_x, min_p_y, max_p_x, max_p_y; + for (auto shape : shapes_) { + shape->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y); + min_x = std::min(min_x, min_p_x); + min_y = std::min(min_y, min_p_y); + max_x = std::max(max_x, max_p_x); + max_y = std::max(max_y, max_p_y); + } + + if ( + min_x == std::numeric_limits::max() || + min_y == std::numeric_limits::max() || + max_x == std::numeric_limits::lowest() || + max_y == std::numeric_limits::lowest()) + { + throw std::runtime_error("Can not obtain map boundaries"); + } +} + +void VectorObjectServer::updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y) +{ + // Calculate size of update map + int size_x = static_cast((max_x - min_x) / resolution_) + 1; + int size_y = static_cast((max_y - min_y) / resolution_) + 1; + + if (size_x < 0) { + throw std::runtime_error("Incorrect map x-size"); + } + + if (size_y < 0) { + throw std::runtime_error("Incorrect map y-size"); + } + + if (!map_) { + map_ = std::make_shared(); + } + + if ( + map_->info.width != static_cast(size_x) || + map_->info.height != static_cast(size_y)) + { + // Map size was changed + map_->data = std::vector(size_x * size_y, default_value_); + map_->info.width = size_x; + map_->info.height = size_y; + } else if (size_x > 0 && size_y > 0) { + // Map size was not changed + memset(map_->data.data(), default_value_, size_x * size_y * sizeof(int8_t)); + } + + map_->header.frame_id = global_frame_id_; + map_->info.resolution = resolution_; + map_->info.origin.position.x = min_x; + map_->info.origin.position.y = min_y; +} + +void VectorObjectServer::putVectorObjectsOnMap() +{ + // Filling the shapes + for (auto shape : shapes_) { + if (shape->isFill()) { + // Put filled shape on map + double wx1 = std::numeric_limits::max(); + double wy1 = std::numeric_limits::max(); + double wx2 = std::numeric_limits::lowest(); + double wy2 = std::numeric_limits::lowest(); + unsigned int mx1 = 0; + unsigned int my1 = 0; + unsigned int mx2 = 0; + unsigned int my2 = 0; + + shape->getBoundaries(wx1, wy1, wx2, wy2); + if ( + !nav2_util::worldToMap(map_, wx1, wy1, mx1, my1) || + !nav2_util::worldToMap(map_, wx2, wy2, mx2, my2)) + { + RCLCPP_ERROR( + get_logger(), + "Error to get shape boundaries on map (UUID: %s)", shape->getUUID().c_str()); + return; + } + + unsigned int it; + for (unsigned int my = my1; my <= my2; my++) { + for (unsigned int mx = mx1; mx <= mx2; mx++) { + it = my * map_->info.width + mx; + double wx, wy; + nav2_util::mapToWorld(map_, mx, my, wx, wy); + if (shape->isPointInside(wx, wy)) { + processVal(map_->data[it], shape->getValue(), overlay_type_); + } + } + } + } else { + // Put shape borders on map + shape->putBorders(map_, overlay_type_); + } + } +} + +void VectorObjectServer::publishMap() +{ + if (map_) { + auto map = std::make_unique(*map_); + map_pub_->publish(std::move(map)); + } +} + +void VectorObjectServer::processMap() +{ + if (!process_map_) { + return; + } + + try { + if (shapes_.size() > 0) { + if (!transformVectorObjects()) { + return; + } + double min_x, min_y, max_x, max_y; + + getMapBoundaries(min_x, min_y, max_x, max_y); + updateMap(min_x, min_y, max_x, max_y); + putVectorObjectsOnMap(); + } else { + updateMap(0.0, 0.0, 0.0, 0.0); + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Can not update map: %s", ex.what()); + return; + } + + publishMap(); +} + +void VectorObjectServer::switchMapUpdate() +{ + for (auto shape : shapes_) { + if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) { + if (!map_timer_) { + map_timer_ = this->create_timer( + std::chrono::duration(1.0 / update_frequency_), + std::bind(&VectorObjectServer::processMap, this)); + } + RCLCPP_INFO(get_logger(), "Publishing map dynamically at %f Hz rate", update_frequency_); + return; + } + } + + if (map_timer_) { + map_timer_->cancel(); + map_timer_.reset(); + } + RCLCPP_INFO(get_logger(), "Publishing map once"); + processMap(); +} + +void VectorObjectServer::addShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // Initialize result with true. If one of the required vector object was not added properly, + // set it to false. + response->success = true; + + auto node = shared_from_this(); + + // Process polygons + for (auto req_poly : request->polygons) { + nav2_msgs::msg::PolygonObject::SharedPtr new_params = + std::make_shared(req_poly); + + auto it = findShape(new_params->uuid.uuid.data()); + if (it != shapes_.end()) { + // Vector Object with given UUID was found: updating it + // Check that found shape has correct type + if ((*it)->getType() != POLYGON) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a polygon type for a polygon update. Not adding shape.", + (*it)->getUUID().c_str()); + response->success = false; + // Do not add this shape + continue; + } + + std::shared_ptr polygon = std::static_pointer_cast(*it); + + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::PolygonObject::SharedPtr old_params = polygon->getParams(); + if (!polygon->setParams(new_params)) { + RCLCPP_ERROR( + get_logger(), + "Failed to update existing polygon object (UUID: %s) with new params. " + "Reverting to old polygon params.", + (*it)->getUUID().c_str()); + // Restore old parameters + polygon->setParams(old_params); + // ... and set the failure to return + response->success = false; + } + } else { + // Vector Object with given UUID was not found: creating a new one + std::shared_ptr polygon = std::make_shared(node); + if (polygon->setParams(new_params)) { + shapes_.push_back(polygon); + } else { + RCLCPP_ERROR( + get_logger(), "Failed to create a new polygon object using the provided params."); + response->success = false; + } + } + } + + // Process circles + for (auto req_crcl : request->circles) { + nav2_msgs::msg::CircleObject::SharedPtr new_params = + std::make_shared(req_crcl); + + auto it = findShape(new_params->uuid.uuid.data()); + if (it != shapes_.end()) { + // Vector object with given UUID was found: updating it + // Check that found shape has correct type + if ((*it)->getType() != CIRCLE) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a circle type for a circle update. Not adding shape.", + (*it)->getUUID().c_str()); + response->success = false; + // Do not add this shape + continue; + } + + std::shared_ptr circle = std::static_pointer_cast(*it); + + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::CircleObject::SharedPtr old_params = circle->getParams(); + if (!circle->setParams(new_params)) { + RCLCPP_ERROR( + get_logger(), + "Failed to update existing circle object (UUID: %s) with new params. " + "Reverting to old circle params.", + (*it)->getUUID().c_str()); + // Restore old parameters + circle->setParams(old_params); + // ... and set the failure to return + response->success = false; + } + } else { + // Vector Object with given UUID was not found: creating a new one + std::shared_ptr circle = std::make_shared(node); + if (circle->setParams(new_params)) { + shapes_.push_back(circle); + } else { + RCLCPP_ERROR( + get_logger(), "Failed to create a new circle object using the provided params."); + response->success = false; + } + } + } + + switchMapUpdate(); +} + +void VectorObjectServer::getShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + std::shared_ptr polygon; + std::shared_ptr circle; + + for (auto shape : shapes_) { + switch (shape->getType()) { + case POLYGON: + polygon = std::static_pointer_cast(shape); + response->polygons.push_back(*(polygon->getParams())); + break; + case CIRCLE: + circle = std::static_pointer_cast(shape); + response->circles.push_back(*(circle->getParams())); + break; + default: + RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape->getUUID().c_str()); + } + } +} + +void VectorObjectServer::removeShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // Initialize result with true. If one of the required vector object was not found, + // set it to false. + response->success = true; + + if (request->all_objects) { + // Clear all objects + shapes_.clear(); + } else { + // Find objects to remove + for (auto req_uuid : request->uuids) { + auto it = findShape(req_uuid.uuid.data()); + if (it != shapes_.end()) { + // Shape with given UUID was found: remove it + (*it).reset(); + shapes_.erase(it); + } else { + // Required vector object was not found + RCLCPP_ERROR( + get_logger(), + "Can not find shape to remove with UUID: %s", + unparseUUID(req_uuid.uuid.data()).c_str()); + response->success = false; + } + } + } + + switchMapUpdate(); +} + +} // namespace nav2_map_server + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::VectorObjectServer) diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp new file mode 100644 index 00000000000..72466486c01 --- /dev/null +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -0,0 +1,559 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 "nav2_map_server/vector_object_shapes.hpp" + +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "nav2_util/occ_grid_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/raytrace_line_2d.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace nav2_map_server +{ + +// ---------- Shape ---------- + +Shape::Shape(const nav2::LifecycleNode::WeakPtr & node) +: type_(UNKNOWN), node_(node) +{} + +Shape::~Shape() +{} + +ShapeType Shape::getType() +{ + return type_; +} + +bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + try { + // Try to get shape UUID from ROS-parameters + std::string uuid_str = nav2::declare_or_get_parameter( + node, shape_name + ".uuid", rclcpp::PARAMETER_STRING); + if (uuid_parse(uuid_str.c_str(), out_uuid) != 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Can not parse UUID string for shape: %s", + shape_name.c_str(), uuid_str.c_str()); + return false; + } + } catch (const std::exception &) { + // If no UUID was specified, generate a new one + uuid_generate(out_uuid); + + char uuid_str[37]; + uuid_unparse(out_uuid, uuid_str); + RCLCPP_INFO( + node->get_logger(), + "[%s] No UUID is specified for shape. Generating a new one: %s", + shape_name.c_str(), uuid_str); + } + + return true; +} + +// ---------- Polygon ---------- + +Polygon::Polygon( + const nav2::LifecycleNode::WeakPtr & node) +: Shape::Shape(node) +{ + type_ = POLYGON; +} + +int8_t Polygon::getValue() const +{ + return params_->value; +} + +std::string Polygon::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Polygon::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +bool Polygon::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Polygon::isFill() const +{ + return params_->closed; +} + +bool Polygon::obtainParams(const std::string & shape_name) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!params_) { + params_ = std::make_shared(); + } + if (!polygon_) { + polygon_ = std::make_shared(); + } + + params_->header.frame_id = nav2::declare_or_get_parameter( + node, shape_name + ".frame_id", std::string{"map"}); + params_->value = nav2::declare_or_get_parameter( + node, shape_name + ".value", static_cast(nav2_util::OCC_GRID_OCCUPIED)); + params_->closed = nav2::declare_or_get_parameter( + node, shape_name + ".closed", true); + + std::vector poly_row; + try { + poly_row = nav2::declare_or_get_parameter>( + node, shape_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Error while getting polygon parameters: %s", + shape_name.c_str(), ex.what()); + return false; + } + // Check for points format correctness + if (poly_row.size() < 6 || poly_row.size() % 2 != 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Polygon has incorrect points description", + shape_name.c_str()); + return false; + } + + // Obtain polygon vertices + geometry_msgs::msg::Point32 point; + bool first = true; + for (double val : poly_row) { + if (first) { + point.x = val; + } else { + point.y = val; + params_->points.push_back(point); + } + first = !first; + } + + // Filling the polygon_ with obtained points in map's frame + polygon_->points = params_->points; + + // Getting shape UUID + return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); +} + +nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const +{ + return params_; +} + +bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) +{ + params_ = params; + + if (!polygon_) { + polygon_ = std::make_shared(); + } + polygon_->points = params_->points; + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } + + return checkConsistency(); +} + +bool Polygon::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + for (unsigned int i = 0; i < params_->points.size(); i++) { + from_pose.pose.position.x = params_->points[i].x; + from_pose.pose.position.y = params_->points[i].y; + from_pose.pose.position.z = params_->points[i].z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + polygon_->points[i].x = to_pose.pose.position.x; + polygon_->points[i].y = to_pose.pose.position.y; + polygon_->points[i].z = to_pose.pose.position.z; + } else { + return false; + } + } + + return true; +} + +void Polygon::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) +{ + min_x = std::numeric_limits::max(); + min_y = std::numeric_limits::max(); + max_x = std::numeric_limits::lowest(); + max_y = std::numeric_limits::lowest(); + + for (auto point : polygon_->points) { + min_x = std::min(min_x, static_cast(point.x)); + min_y = std::min(min_y, static_cast(point.y)); + max_x = std::max(max_x, static_cast(point.x)); + max_y = std::max(max_y, static_cast(point.y)); + } +} + +bool Polygon::isPointInside(const double px, const double py) const +{ + return nav2_util::geometry_utils::isPointInsidePolygon(px, py, polygon_->points); +} + +void Polygon::putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) +{ + unsigned int mx0, my0, mx1, my1; + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!nav2_util::worldToMap(map, polygon_->points[0].x, polygon_->points[0].y, mx1, my1)) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), polygon_->points[0].x, polygon_->points[0].y); + return; + } + + MapAction ma(map, params_->value, overlay_type); + for (unsigned int i = 1; i < polygon_->points.size(); i++) { + mx0 = mx1; + my0 = my1; + if (!nav2_util::worldToMap(map, polygon_->points[i].x, polygon_->points[i].y, mx1, my1)) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), polygon_->points[i].x, polygon_->points[i].y); + return; + } + nav2_util::raytraceLine(ma, mx0, my0, mx1, my1, map->info.width); + } +} + +bool Polygon::checkConsistency() +{ + if (params_->points.size() < 3) { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Polygon has incorrect number of vertices: %li", + getUUID().c_str(), params_->points.size()); + return false; + } + + return true; +} + +// ---------- Circle ---------- + +Circle::Circle( + const nav2::LifecycleNode::WeakPtr & node) +: Shape::Shape(node) +{ + type_ = CIRCLE; +} + +int8_t Circle::getValue() const +{ + return params_->value; +} + +std::string Circle::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Circle::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +bool Circle::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Circle::isFill() const +{ + return params_->fill; +} + +bool Circle::obtainParams(const std::string & shape_name) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!params_) { + params_ = std::make_shared(); + } + if (!center_) { + center_ = std::make_shared(); + } + + params_->header.frame_id = nav2::declare_or_get_parameter( + node, shape_name + ".frame_id", std::string{"map"}); + params_->value = nav2::declare_or_get_parameter( + node, shape_name + ".value", static_cast(nav2_util::OCC_GRID_OCCUPIED)); + params_->fill = nav2::declare_or_get_parameter( + node, shape_name + ".fill", true); + + std::vector center_row; + try { + center_row = nav2::declare_or_get_parameter>( + node, shape_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY); + params_->radius = nav2::declare_or_get_parameter( + node, shape_name + ".radius", rclcpp::PARAMETER_DOUBLE); + if (params_->radius < 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Circle has incorrect radius less than zero", + shape_name.c_str()); + return false; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Error while getting circle parameters: %s", + shape_name.c_str(), ex.what()); + return false; + } + // Check for points format correctness + if (center_row.size() != 2) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Circle has incorrect center description", + shape_name.c_str()); + return false; + } + + // Obtain circle center + params_->center.x = center_row[0]; + params_->center.y = center_row[1]; + // Setting the center_ with obtained circle center in map's frame + *center_ = params_->center; + + // Getting shape UUID + return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); +} + +nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const +{ + return params_; +} + +bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) +{ + params_ = params; + + if (!center_) { + center_ = std::make_shared(); + } + *center_ = params_->center; + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } + + return checkConsistency(); +} + +bool Circle::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + from_pose.pose.position.x = params_->center.x; + from_pose.pose.position.y = params_->center.y; + from_pose.pose.position.z = params_->center.z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + center_->x = to_pose.pose.position.x; + center_->y = to_pose.pose.position.y; + center_->z = to_pose.pose.position.z; + } else { + return false; + } + + return true; +} + +void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) +{ + min_x = center_->x - params_->radius; + min_y = center_->y - params_->radius; + max_x = center_->x + params_->radius; + max_y = center_->y + params_->radius; +} + +bool Circle::isPointInside(const double px, const double py) const +{ + return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <= + params_->radius * params_->radius; +} + +void Circle::putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) +{ + unsigned int mcx, mcy; + if (!centerToMap(map, mcx, mcy)) { + return; + } + + // Implementation of the circle generation algorithm, based on the following work: + // Berthold K.P. Horn "Circle generators for display devices" + // Computer Graphics and Image Processing 5.2 (1976): 280-288. + + // Inputs initialization + const int r = static_cast(std::round(params_->radius / map->info.resolution)); + int x = r; + int y = 1; + + // Error initialization + int s = -r; + + // Calculation algorithm + while (x > y) { // Calculating only first circle octant + // Put 8 points in each octant reflecting symmetrically + putPoint(mcx + x, mcy + y, map, overlay_type); + putPoint(mcx + y, mcy + x, map, overlay_type); + putPoint(mcx - x + 1, mcy + y, map, overlay_type); + putPoint(mcx + y, mcy - x + 1, map, overlay_type); + putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type); + putPoint(mcx - y + 1, mcy - x + 1, map, overlay_type); + putPoint(mcx + x, mcy - y + 1, map, overlay_type); + putPoint(mcx - y + 1, mcy + x, map, overlay_type); + + s = s + 2 * y + 1; + y++; + if (s > 0) { + s = s - 2 * x + 2; + x--; + } + } + + // Corner case for x == y: do not put end points twice + if (x == y) { + putPoint(mcx + x, mcy + y, map, overlay_type); + putPoint(mcx - x + 1, mcy + y, map, overlay_type); + putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type); + putPoint(mcx + x, mcy - y + 1, map, overlay_type); + } +} + +bool Circle::checkConsistency() +{ + if (params_->radius < 0.0) { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Circle has incorrect radius less than zero", + getUUID().c_str()); + return false; + } + return true; +} + +bool Circle::centerToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + unsigned int & mcx, unsigned int & mcy) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Get center of circle in map coordinates + if (center_->x < map->info.origin.position.x || center_->y < map->info.origin.position.y) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) circle center to map", + getUUID().c_str(), center_->x, center_->y); + return false; + } + // We need the circle center to be always shifted one cell less its logical center + // and to avoid any FP-accuracy losing on small values, so we are using another + // than nav2_util::worldToMap() approach + mcx = static_cast( + std::round((center_->x - map->info.origin.position.x) / map->info.resolution)) - 1; + mcy = static_cast( + std::round((center_->y - map->info.origin.position.y) / map->info.resolution)) - 1; + if (mcx >= map->info.width || mcy >= map->info.height) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), center_->x, center_->y); + return false; + } + + return true; +} + +inline void Circle::putPoint( + unsigned int mx, unsigned int my, + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const OverlayType overlay_type) +{ + processCell(map, my * map->info.width + mx, params_->value, overlay_type); +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt index 4140b14d6f0..06a4b2228a0 100644 --- a/nav2_map_server/test/unit/CMakeLists.txt +++ b/nav2_map_server/test/unit/CMakeLists.txt @@ -18,3 +18,25 @@ target_link_libraries(test_costmap_filter_info_server ${library_name} ${map_io_library_name} ) + +# test_vector_object_shapes unit test +ament_add_gtest(test_vector_object_shapes + test_vector_object_shapes.cpp +) + +target_link_libraries(test_vector_object_shapes ${vo_server_dependencies}) + +target_link_libraries(test_vector_object_shapes + ${vo_library_name} +) + +# test_vector_object_server unit test +ament_add_gtest(test_vector_object_server + test_vector_object_server.cpp +) + +target_link_libraries(test_vector_object_server ${vo_server_dependencies}) + +target_link_libraries(test_vector_object_server + ${vo_library_name} +) diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp index 4fcd8aec35e..0af3f159969 100644 --- a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -134,8 +134,10 @@ class InfoServerTester : public ::testing::Test TEST_F(InfoServerTester, testCostmapFilterInfoPublish) { rclcpp::Time start_time = info_server_->now(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(info_server_->get_node_base_interface()); while (!isReceived()) { - rclcpp::spin_some(info_server_->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(100ms); // Waiting no more than 5 seconds ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); @@ -155,8 +157,10 @@ TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate) info_server_->activate(); rclcpp::Time start_time = info_server_->now(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(info_server_->get_node_base_interface()); while (!isReceived()) { - rclcpp::spin_some(info_server_->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(100ms); // Waiting no more than 5 seconds ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); diff --git a/nav2_map_server/test/unit/test_vector_object_server.cpp b/nav2_map_server/test/unit/test_vector_object_server.cpp new file mode 100644 index 00000000000..26ea6ea7d95 --- /dev/null +++ b/nav2_map_server/test/unit/test_vector_object_server.cpp @@ -0,0 +1,1351 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "unique_identifier_msgs/msg/uuid.hpp" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" + +#include "nav2_msgs/srv/add_shapes.hpp" +#include "nav2_msgs/srv/get_shapes.hpp" +#include "nav2_msgs/srv/remove_shapes.hpp" +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_util/occ_grid_utils.hpp" + +#include "nav2_map_server/vector_object_server.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char POLYGON_NAME[]{"Polygon"}; +static const char CIRCLE_NAME[]{"Circle"}; +static const char GLOBAL_FRAME_ID[]{"map"}; +static const char SHAPE_FRAME_ID[]{"shape"}; +static const double UPDATE_FREQUENCY{10.0}; +static const int8_t POLYGON_VAL{nav2_util::OCC_GRID_OCCUPIED}; +static const int8_t CIRCLE_VAL{nav2_util::OCC_GRID_OCCUPIED / 2}; + +class VOServerWrapper : public nav2_map_server::VectorObjectServer +{ +public: + void start() + { + ASSERT_EQ(on_configure(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_activate(get_current_state()), nav2::CallbackReturn::SUCCESS); + } + + void configureFail() + { + ASSERT_EQ(on_configure(get_current_state()), nav2::CallbackReturn::FAILURE); + } + + void stop() + { + ASSERT_EQ(on_deactivate(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_cleanup(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2::CallbackReturn::SUCCESS); + } + + void cleanup() + { + ASSERT_EQ(on_cleanup(get_current_state()), nav2::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2::CallbackReturn::SUCCESS); + } + + void setProcessMap(const bool process_map) + { + process_map_ = process_map; + } + + void getMapBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) const + { + VectorObjectServer::getMapBoundaries(min_x, min_y, max_x, max_y); + } + + void updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y) + { + VectorObjectServer::updateMap(min_x, min_y, max_x, max_y); + } + + void putVectorObjectsOnMap() + { + VectorObjectServer::putVectorObjectsOnMap(); + } +}; // VOServerWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + + void setVOServerParams(); + void setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid); + void setPolygonParams(const std::string & uuid); + void setCircleParams(const std::string & uuid); + + nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject( + const std::vector & uuid); + nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject( + const std::vector & uuid); + + void sendTransform(const double frame_shift); + + template + typename T::Response::SharedPtr sendRequest( + typename nav2::ServiceClient::SharedPtr client, + typename T::Request::SharedPtr request, + const std::chrono::nanoseconds & timeout); + + void mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map); + bool waitMap(const std::chrono::nanoseconds & timeout); + void verifyMap(bool is_circle, double poly_x_end = 1.0, double circle_cx = 3.0); + + void comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2); + void compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2); + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Service clients for calling AddShapes.srv, GetShapes.srv, RemoveShapes.srv + nav2::ServiceClient::SharedPtr add_shapes_client_; + nav2::ServiceClient::SharedPtr get_shapes_client_; + nav2::ServiceClient::SharedPtr remove_shapes_client_; + + // Output map subscriber + rclcpp::Subscription::SharedPtr vo_map_sub_; + // Output map published by VectorObjectServer + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + + // Vector Object server node + std::shared_ptr vo_server_; + rclcpp::executors::SingleThreadedExecutor executor_; +}; // Tester + +Tester::Tester() +: map_(nullptr) +{ + vo_server_ = std::make_shared(); + + add_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/add_shapes"); + + get_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/get_shapes"); + + remove_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/remove_shapes"); + + vo_map_sub_ = vo_server_->create_subscription( + "vo_map", std::bind(&Tester::mapCallback, this, std::placeholders::_1), + nav2::qos::LatchedSubscriptionQoS()); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(vo_server_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); + executor_.add_node(vo_server_->get_node_base_interface()); +} + +Tester::~Tester() +{ + vo_map_sub_.reset(); + + add_shapes_client_.reset(); + get_shapes_client_.reset(); + remove_shapes_client_.reset(); + + vo_server_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setVOServerParams() +{ + vo_server_->declare_parameter( + "map_topic", rclcpp::ParameterValue("vo_map")); + vo_server_->set_parameter( + rclcpp::Parameter("map_topic", "vo_map")); + + vo_server_->declare_parameter( + "global_frame_id", rclcpp::ParameterValue("map")); + vo_server_->set_parameter( + rclcpp::Parameter("global_frame_id", "map")); + + vo_server_->declare_parameter( + "resolution", rclcpp::ParameterValue(0.1)); + vo_server_->set_parameter( + rclcpp::Parameter("resolution", 0.1)); + + vo_server_->declare_parameter( + "default_value", rclcpp::ParameterValue(nav2_util::OCC_GRID_UNKNOWN)); + vo_server_->set_parameter( + rclcpp::Parameter("default_value", nav2_util::OCC_GRID_UNKNOWN)); + + vo_server_->declare_parameter( + "overlay_type", + rclcpp::ParameterValue(static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + + vo_server_->declare_parameter( + "update_frequency", rclcpp::ParameterValue(UPDATE_FREQUENCY)); + vo_server_->set_parameter( + rclcpp::Parameter("update_frequency", UPDATE_FREQUENCY)); + + vo_server_->declare_parameter( + "transform_tolerance", rclcpp::ParameterValue(0.1)); + vo_server_->set_parameter( + rclcpp::Parameter("transform_tolerance", 0.1)); +} + +void Tester::setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid) +{ + vo_server_->declare_parameter( + "shapes", rclcpp::ParameterValue(std::vector{POLYGON_NAME, CIRCLE_NAME})); + vo_server_->set_parameter( + rclcpp::Parameter("shapes", std::vector{POLYGON_NAME, CIRCLE_NAME})); + + setPolygonParams(poly_uuid); + setCircleParams(circle_uuid); +} + +void Tester::setPolygonParams(const std::string & uuid) +{ + const std::string polygon_name(POLYGON_NAME); + + vo_server_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + vo_server_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + vo_server_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + vo_server_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(POLYGON_VAL)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", POLYGON_VAL)); + + std::vector points{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0}; + vo_server_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + + if (!uuid.empty()) { + vo_server_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue(uuid)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", uuid)); + } +} + +void Tester::setCircleParams(const std::string & uuid) +{ + const std::string circle_name(CIRCLE_NAME); + + vo_server_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + vo_server_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + vo_server_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + vo_server_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(CIRCLE_VAL)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".value", CIRCLE_VAL)); + + vo_server_->declare_parameter( + circle_name + ".center", rclcpp::ParameterValue(std::vector{3.0, 0.0})); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".center", std::vector{3.0, 0.0})); + + vo_server_->declare_parameter( + circle_name + ".radius", rclcpp::ParameterValue(1.0)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + if (!uuid.empty()) { + vo_server_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue(uuid)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", uuid)); + } +} + +void Tester::sendTransform(const double frame_shift) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(vo_server_); + + geometry_msgs::msg::TransformStamped transform; + + // global_frame -> shape_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = SHAPE_FRAME_ID; + + transform.header.stamp = vo_server_->now(); + transform.transform.translation.x = frame_shift; + transform.transform.translation.y = 0.0; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject( + const std::vector & uuid) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared(); + po->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + po->uuid.uuid = uuid_array; + } + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = 1.0; + po->points.push_back(p); + p.x = -1.0; + po->points.push_back(p); + p.y = -1.0; + po->points.push_back(p); + p.x = 1.0; + po->points.push_back(p); + po->closed = true; + po->value = POLYGON_VAL; + + return po; +} + +nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject( + const std::vector & uuid) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared(); + co->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + co->uuid.uuid = uuid_array; + } + co->center.x = 3.0; + co->center.y = 0.0; + co->radius = 1.0; + co->fill = true; + co->value = CIRCLE_VAL; + + return co; +} + +template +typename T::Response::SharedPtr Tester::sendRequest( + typename nav2::ServiceClient::SharedPtr client, + typename T::Request::SharedPtr request, + const std::chrono::nanoseconds & timeout) +{ + auto result_future = client->async_call(request); + + rclcpp::Time start_time = vo_server_->now(); + while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return result_future.get(); + } + executor_.spin_some(); + std::this_thread::sleep_for(10ms); + } + return nullptr; +} + +void Tester::mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + map_ = map; +} + +bool Tester::waitMap(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = vo_server_->now(); + while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) { + if (map_) { + return true; + } + executor_.spin_some(); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::verifyMap(bool is_circle, double poly_x_end, double circle_cx) +{ + // Wait for map to be received + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // Map should contain polygon and circle and will look like: + // + // polygon {x1, y1} (and map's origin): should be always {-1.0, -1.0} + // V + // xxxxxx....xxx. + // xxxxxx...xxxxx < circle is optionally placed on map + // xxxxxx...xxxxx + // xxxxxx....xxx. + // ^ + // polygon {x2, y2}. x2 = poly_x_end; y2 - is always == 1.0 + + // Polygon {x2, y2} in map coordinates + const unsigned int m_poly_x2 = 9 + poly_x_end * 10; + const unsigned int m_poly_y2 = 19; + + // Center of the circle in map coordinates (expressed in floating-point for best precision) + const double m_circle_cx = 9 + circle_cx * 10 + 0.5; // cell's origin + 0.5 center of cell + const double m_circle_cy = 9 + 0.5; // cell's origin + 0.5 center of cell + + // Radius of the circle in map coordinates + const double m_circle_rad = 10.0; + + for (unsigned int my = 0; my < map_->info.height; my++) { + for (unsigned int mx = 0; mx < map_->info.width; mx++) { + if (mx <= m_poly_x2 && my <= m_poly_y2) { + // Point belongs to the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + } else if (is_circle && std::hypot(m_circle_cx - mx, m_circle_cy - my) <= m_circle_rad) { + // Point belongs to the circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + } else { + // Point does not belong to any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + } + } + } +} + +void Tester::comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2) +{ + ASSERT_EQ(p1->header.frame_id, p2->header.frame_id); + ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid); + ASSERT_EQ(p1->points.size(), p2->points.size()); + for (unsigned int i = 0; i < p1->points.size(); i++) { + ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON); + ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON); + } + ASSERT_EQ(p1->closed, p2->closed); + ASSERT_EQ(p1->value, p2->value); +} + +void Tester::compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2) +{ + ASSERT_EQ(c1->header.frame_id, c2->header.frame_id); + ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid); + ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON); + ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON); + ASSERT_NEAR(c1->radius, c2->radius, EPSILON); + ASSERT_EQ(c1->fill, c2->fill); + ASSERT_EQ(c1->value, c2->value); +} + +// ---------- ROS-parameters tests ---------- +TEST_F(Tester, testObtainParams) +{ + setVOServerParams(); + setShapesParams( + "01010101-0101-0101-0101-010101010101", + "01010101-0101-0101-0101-010101010102"); + vo_server_->start(); + + verifyMap(true); + + vo_server_->stop(); +} + +TEST_F(Tester, testObtainNoShapes) +{ + setVOServerParams(); + // Set shapes array, but does not set shapes themselves + vo_server_->declare_parameter( + "shapes", rclcpp::ParameterValue(std::vector{POLYGON_NAME, CIRCLE_NAME})); + vo_server_->set_parameter( + rclcpp::Parameter("shapes", std::vector{POLYGON_NAME, CIRCLE_NAME})); + + // Configured that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectShapeType) +{ + setVOServerParams(); + setShapesParams("", ""); + // Setting incorrect type of circle + vo_server_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".type", "incorrect_type")); + + // Configured that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectPolygonParams) +{ + setVOServerParams(); + // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test + setShapesParams( + "incorrect_polygon_uuid", + "01010101-0101-0101-0101-010101010102"); + + // Set that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectCircleParams) +{ + setVOServerParams(); + // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test + setShapesParams( + "01010101-0101-0101-0101-010101010101", + "incorrect_circle_uuid"); + + // Set that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +// ---------- Service call tests ---------- +TEST_F(Tester, testAddShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Now move X-coordinate of polygon's border and + // add a new circle fully placed inside first one + // through AddShapes.srv: + // Update polygon x2-coordinate to 1.5 + po_msg->points[0].x = 1.5; + po_msg->points[3].x = 1.5; + // Prepare second circle object (fully placed inside first circle) + auto co2_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3}); + co2_msg->radius = 0.5; + + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_msg->circles.push_back(*co2_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true, 1.5); + + // Check that getShapes.srv will return correct shapes + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circles + ASSERT_EQ(get_shapes_result->circles.size(), 2u); + c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + c_check = std::make_shared(get_shapes_result->circles[1]); + compareCircleObjects(c_check, co2_msg); + + vo_server_->stop(); +} + +TEST_F(Tester, testRemoveShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Now remove circle from map + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = false; + unique_identifier_msgs::msg::UUID uuid; + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}; + remove_shapes_msg->uuids.push_back(uuid); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Check that there is only polygon remained on map + verifyMap(false); + + // Check that getShapes.srv will return only polygon + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + // Then call RemoveShapes.srv with enabled all_objects parameter + remove_shapes_msg->all_objects = true; + remove_shapes_msg->uuids.clear(); + map_.reset(); // Resetting the map to ensure that updated one is received later + remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Check that map is empty + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN); + + // getShapes.srv should return empty vectors + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + ASSERT_EQ(get_shapes_result->polygons.size(), 0u); + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + vo_server_->stop(); +} + +TEST_F(Tester, testAddIncorrectShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Then try to update polygon with circle's uuid + po_msg->uuid.uuid[15] = 2; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + po_msg->uuid.uuid[15] = 1; // Restoring back for further usage + + // Then try to update circle with polygon's uuid + co_msg->uuid.uuid[15] = 1; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + co_msg->uuid.uuid[15] = 2; // Restoring back for further usage + + // Try to update polygon with incorrect number of points + po_msg->points.resize(2); + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + + // Try to add new incorrect polygon + po_msg->uuid.uuid[15] = 100; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + // Restoring back for further usage + po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + + // Try to update circle with incorrect radius + co_msg->radius = -1.0; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + + // Try to add new incorrect circle + co_msg->uuid.uuid[15] = 100; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + co_msg->radius = 1.0; // Restoring back for further usage + co_msg->uuid.uuid[15] = 2; // Restoring back for further usage + + // Verify that map did not corrupted after all false-manipulations + verifyMap(true); + + // Check that getShapes.srv will return correct shapes after all false-manipulations + get_shapes_msg = std::make_shared(); + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + vo_server_->stop(); +} + +TEST_F(Tester, testRemoveIncorrectShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Then try to remove two shapes: non-existing shape and circle + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = false; + unique_identifier_msgs::msg::UUID uuid; + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 100}; + remove_shapes_msg->uuids.push_back(uuid); + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}; + remove_shapes_msg->uuids.push_back(uuid); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_FALSE(remove_shapes_result->success); + + // Check that despite on the error, the circle was removed from map + verifyMap(false); + + // Check that getShapes.srv will return only polygon + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + vo_server_->stop(); +} + +// ---------- Overlay tests ---------- +TEST_F(Tester, testOverlayMax) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_MAX))); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that overlap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 14; // in the overlapping area + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 20; // inside circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlayMin) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_MIN))); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that overlap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 14; // in the overlapping area + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + mx = 20; // inside circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlaySeq) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + vo_server_->start(); + + // Sequentially add polygon and then circle overlapped with it on map + auto add_shapes_msg = std::make_shared(); + + // Prepare first polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + add_shapes_msg->polygons.push_back(*po_msg); + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Then prepare circle object to add over the polygon + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + // Also check that putBorders part works correctly + co_msg->fill = false; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that overlap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 10; // on the circle border laying over the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + mx = 14; // inside circle and polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 24; // inside circle only + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + mx = 29; // on the circle border laying outside the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlayUnknown) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter("overlay_type", static_cast(1000))); + vo_server_->start(); + + // Try to add polygon on map + auto add_shapes_msg = std::make_shared(); + auto po_msg = makePolygonObject(std::vector()); + add_shapes_msg->polygons.push_back(*po_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Check that polygon was not added on map: map is empty + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +// ---------- Transform/dynamic tests ---------- +TEST_F(Tester, testShapesMove) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // No shift between polygon and map + sendTransform(0.0); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Polygon and circle to be in another moving frame + po_msg->header.frame_id = SHAPE_FRAME_ID; + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that polygon and circle are in correct positions on map + verifyMap(true); + + // Move frame with polygon + sendTransform(0.5); + + // Map is being published dynamically. Wait for the map to be published one more time + map_.reset(); + // Check that polygon and circle are in correct positions on map + verifyMap(true, 1.0, 2.5); // Polygon is moved 0.5m towards to the circle + // Check that map's origin was updated in accordance to polygon movement + ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON); // -1.0 + 0.5 + ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON); + + // Then try to publish polygon in incorrect frame + add_shapes_msg->polygons[0].header.frame_id = "incorrect_frame"; + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Having incorrect transform, map should not be published + ASSERT_FALSE(waitMap(timeout)); + + vo_server_->stop(); +} + +TEST_F(Tester, testSwitchDynamicStatic) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // 0.5m shift between polygon and map + sendTransform(0.5); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Polygon to be in different frame + po_msg->header.frame_id = SHAPE_FRAME_ID; + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that polygon and circle are in correct positions on map + verifyMap(true, 1.0, 2.5); // Polygon is moved 0.5m towards to the circle + // Check that map's origin was updated in accordance to polygon movement + ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON); // -1.0 + 0.5 + ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON); + + // Then return to the static model and ensure the everything works correctly + add_shapes_msg->polygons[0].header.frame_id = GLOBAL_FRAME_ID; + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + verifyMap(true); + + vo_server_->stop(); +} + +// ---------- Corner cases ---------- +TEST_F(Tester, switchProcessMap) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // Check that received default map is empty + ASSERT_EQ(map_->header.frame_id, "map"); + ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON); + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + + // Disable map processing and trying to obtain the map + vo_server_->setProcessMap(false); + + // Switching map update by calling remove service + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = true; + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Having process_map_ disabled, map should not be published + ASSERT_FALSE(waitMap(timeout)); + + // Then enable map processing and trying to obtain the map + vo_server_->setProcessMap(true); + + // Switching map update by calling remove service + map_.reset(); // Resetting the map to ensure that updated one is received later + remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Ensure that map is being updated + ASSERT_TRUE(waitMap(timeout)); + + // ... and it is empty + ASSERT_EQ(map_->header.frame_id, "map"); + ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON); + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + + vo_server_->stop(); +} + +TEST_F(Tester, testIncorrectMapBoundaries) { + setVOServerParams(); + vo_server_->start(); + + // Set min_x > max_x + EXPECT_THROW(vo_server_->updateMap(1.0, 0.1, 0.1, 1.0), std::runtime_error); + + // Set min_y > max_y + EXPECT_THROW(vo_server_->updateMap(0.1, 1.0, 1.0, 0.1), std::runtime_error); +} + +TEST_F(Tester, testIncorrectMapBoundariesWhenNoShapes) { + setVOServerParams(); + vo_server_->start(); + double min_x, min_y, max_x, max_y; + EXPECT_THROW(vo_server_->getMapBoundaries(min_x, min_y, max_x, max_y), std::runtime_error); +} + +TEST_F(Tester, testShapeOutsideMap) { + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true); + + // Modify the map to have a shape outside the map + vo_server_->updateMap(2.0, 2.0, 4.0, 4.0); + + // Try to put the shapes back in the map + vo_server_->putVectorObjectsOnMap(); + + // Verify that map did not corrupted after all false-manipulations + verifyMap(true); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_map_server/test/unit/test_vector_object_shapes.cpp b/nav2_map_server/test/unit/test_vector_object_shapes.cpp new file mode 100644 index 00000000000..43a6f43a58d --- /dev/null +++ b/nav2_map_server/test/unit/test_vector_object_shapes.cpp @@ -0,0 +1,805 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/transform_broadcaster.hpp" + +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_util/occ_grid_utils.hpp" + +#include "nav2_map_server/vector_object_shapes.hpp" +#include "nav2_map_server/vector_object_utils.hpp" + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char POLYGON_NAME[]{"Polygon"}; +static const char CIRCLE_NAME[]{"Circle"}; +static const char GLOBAL_FRAME_ID[]{"map"}; +static const char SHAPE_FRAME_ID[]{"shape"}; +static double FRAME_SHIFT = 0.1; +static std::vector POINTS{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0}; +static std::vector CENTER{0.0, 0.0}; + +class PolygonWrapper : public nav2_map_server::Polygon +{ +public: + explicit PolygonWrapper(const nav2::LifecycleNode::WeakPtr & node) + : Polygon(node) + {} + + geometry_msgs::msg::Polygon::SharedPtr getPoly() + { + return polygon_; + } +}; // PolygonWrapper + +class CircleWrapper : public nav2_map_server::Circle +{ +public: + explicit CircleWrapper(const nav2::LifecycleNode::WeakPtr & node) + : Circle(node) + {} + + geometry_msgs::msg::Point32::SharedPtr getCenter() + { + return center_; + } +}; // CircleWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + +protected: + void setPolygonParams(const std::string & uuid); + void setCircleParams(const std::string & uuid); + nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject( + const std::vector & uuid); + nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject( + const std::vector & uuid); + + void sendTransform(); + + nav_msgs::msg::OccupancyGrid::SharedPtr makeMap(); + void verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map); + void verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map); + void verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map); + + void comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2); + void compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2); + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::shared_ptr polygon_; + std::shared_ptr circle_; + + nav2::LifecycleNode::SharedPtr node_; +}; // Tester + +Tester::Tester() +{ + node_ = std::make_shared("test_node"); + + // Create shapes + polygon_ = std::make_shared(node_); + circle_ = std::make_shared(node_); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + polygon_.reset(); + circle_.reset(); + + node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setPolygonParams(const std::string & uuid) +{ + const std::string polygon_name(POLYGON_NAME); + + node_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + node_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + node_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(POINTS)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", POINTS)); + + if (!uuid.empty()) { + node_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue(uuid)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", uuid)); + } +} + +void Tester::setCircleParams(const std::string & uuid) +{ + const std::string circle_name(CIRCLE_NAME); + + node_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + node_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + node_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + circle_name + ".center", rclcpp::ParameterValue(CENTER)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + node_->declare_parameter( + circle_name + ".radius", rclcpp::ParameterValue(1.0)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + if (!uuid.empty()) { + node_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue(uuid)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", uuid)); + } +} + +nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject( + const std::vector & uuid) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared(); + po->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + po->uuid.uuid = uuid_array; + } + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = 1.0; + po->points.push_back(p); + p.x = -1.0; + po->points.push_back(p); + p.y = -1.0; + po->points.push_back(p); + p.x = 1.0; + po->points.push_back(p); + po->closed = true; + po->value = nav2_util::OCC_GRID_OCCUPIED; + + return po; +} + +nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject( + const std::vector & uuid) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared(); + co->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + co->uuid.uuid = uuid_array; + } + co->center.x = 0.0; + co->center.y = 0.0; + co->radius = 1.0; + co->fill = true; + co->value = nav2_util::OCC_GRID_OCCUPIED; + + return co; +} + +void Tester::sendTransform() +{ + std::shared_ptr tf_broadcaster = + std::make_shared(node_); + + geometry_msgs::msg::TransformStamped transform; + + // global_frame -> shape_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = SHAPE_FRAME_ID; + + transform.header.stamp = node_->now(); + transform.transform.translation.x = FRAME_SHIFT; + transform.transform.translation.y = FRAME_SHIFT; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +nav_msgs::msg::OccupancyGrid::SharedPtr Tester::makeMap() +{ + nav_msgs::msg::OccupancyGrid::SharedPtr map = std::make_shared(); + map->header.frame_id = GLOBAL_FRAME_ID; + map->info.resolution = 0.1; + map->info.width = 40; + map->info.height = 40; + map->info.origin.position.x = -2.0; + map->info.origin.position.y = -2.0; + map->data = std::vector(400 * 400, nav2_util::OCC_GRID_FREE); + + return map; +} + +void Tester::verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + // Map is expected to be as follows: + // 0 0 0 0 0 0 0 0 + // 0 0 x x x x 0 0 + // 0 0 x 0 0 0 0 0 + // 0 0 x 0 0 0 0 0 + // 0 0 x x x x 0 0 + // 0 0 0 0 0 0 0 0 + const unsigned int map_center_x = 19; + const unsigned int map_center_y = 19; + + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + if (my == map_center_y - 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) { + // 1st border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else if (mx == map_center_x - 10 && my >= map_center_y - 10 && my <= map_center_y + 10) { + // 2nd border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else if (my == map_center_y + 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) { + // 3rd border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else { + // Does not belong to any border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE); + } + } + } +} + +void Tester::verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + // Circle center in cell coordinates + const double circle_center_x = 19.5; // 19 cell's origin + 0.5 center of cell + const double circle_center_y = 19.5; // 19 cell's origin + 0.5 center of cell + + double radius; + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + if (map->data[my * map->info.width + mx] == nav2_util::OCC_GRID_OCCUPIED) { + radius = std::hypot(circle_center_x - mx, circle_center_y - my); + ASSERT_NEAR(radius, 10.0, 1.0); // Border drift no more than once cell + } + } + } +} + +void Tester::verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE); + } + } +} + +void Tester::comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2) +{ + ASSERT_EQ(p1->header.frame_id, p2->header.frame_id); + ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid); + ASSERT_EQ(p1->points.size(), p2->points.size()); + for (unsigned int i = 0; i < p1->points.size(); i++) { + ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON); + ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON); + } + ASSERT_EQ(p1->closed, p2->closed); + ASSERT_EQ(p1->value, p2->value); +} + +void Tester::compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2) +{ + ASSERT_EQ(c1->header.frame_id, c2->header.frame_id); + ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid); + ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON); + ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON); + ASSERT_NEAR(c1->radius, c2->radius, EPSILON); + ASSERT_EQ(c1->fill, c2->fill); + ASSERT_EQ(c1->value, c2->value); +} + +//---------- Polygon testcases ---------- + +TEST_F(Tester, testPolygonObtainParams) +{ + setPolygonParams("01010101-0101-0101-0101-010101010101"); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON); + ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(polygon_->isFill(), true); + ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-010101010101"); +} + +TEST_F(Tester, testPolygonObtainIncorrectParams) +{ + // Setting polygon parameters w/o points + const std::string polygon_name(POLYGON_NAME); + + node_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + node_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + node_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + polygon_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + + // Check that points is mandatory parameter for the polygon + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); + + // Setting incorrect number of points + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", std::vector{1.0, 2.0, 3.0})); + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", POINTS)); + + // Setting incorrect UUID + node_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", "incorrect-uuid")); + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); +} + +TEST_F(Tester, testPolygonSetParams) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2}); + + // Check that parameters were set correctly + ASSERT_TRUE(polygon_->setParams(po)); + ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON); + ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(polygon_->isFill(), true); + ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-020202020202"); + + // Verify that getting the parameters also works correctly + nav2_msgs::msg::PolygonObject::SharedPtr params = polygon_->getParams(); + comparePolygonObjects(params, po); +} + +TEST_F(Tester, testPolygonSetIncorrectParams) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject( + std::vector()); + // Setting incorrect number of vertices + po->points.resize(2); + + // And check that it is failed to set these parameters + ASSERT_FALSE(polygon_->setParams(po)); +} + +TEST_F(Tester, testPolygonBoundaries) +{ + setPolygonParams(""); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + double min_x, min_y, max_x, max_y; + polygon_->getBoundaries(min_x, min_y, max_x, max_y); + + ASSERT_NEAR(min_x, -1.0, EPSILON); + ASSERT_NEAR(min_y, -1.0, EPSILON); + ASSERT_NEAR(max_x, 1.0, EPSILON); + ASSERT_NEAR(max_y, 1.0, EPSILON); +} + +TEST_F(Tester, testPolygonPoints) +{ + setPolygonParams(""); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + ASSERT_TRUE(polygon_->isPointInside(0.0, 0.0)); + ASSERT_FALSE(polygon_->isPointInside(-2.0, -2.0)); + ASSERT_FALSE(polygon_->isPointInside(2.0, 2.0)); +} + +TEST_F(Tester, testPolygonBorders) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyPolygonBorders(map); +} + +TEST_F(Tester, testPolygonBordersOutOfBoundaries) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + // Set polygon to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + ".points", + std::vector{5.0, 5.0, 6.0, 5.0, 5.0, 6.0})); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testPolygonBordersOnePointInsideBoundaries) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + // Now, set the first point inside the map and the others out of the map + node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + ".points", + std::vector{0.5, 0.5, 6.0, 5.0, 5.0, 6.0})); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testPolygonDifferentFrame) +{ + setPolygonParams(""); + // Change shape's frame ID to differ from global one + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".frame_id", SHAPE_FRAME_ID)); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + sendTransform(); + + // Check that shape coordinates are not transformed + geometry_msgs::msg::Polygon::SharedPtr poly = polygon_->getPoly(); + ASSERT_NEAR(poly->points[0].x, 1.0, EPSILON); + ASSERT_NEAR(poly->points[0].y, 1.0, EPSILON); + ASSERT_NEAR(poly->points[1].x, -1.0, EPSILON); + ASSERT_NEAR(poly->points[1].y, 1.0, EPSILON); + ASSERT_NEAR(poly->points[2].x, -1.0, EPSILON); + ASSERT_NEAR(poly->points[2].y, -1.0, EPSILON); + ASSERT_NEAR(poly->points[3].x, 1.0, EPSILON); + ASSERT_NEAR(poly->points[3].y, -1.0, EPSILON); + + // Transform shape coordinates to global frame + ASSERT_TRUE(polygon_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0)); + + // Verify that shape coordinates were transformed to global frame successfully + poly = polygon_->getPoly(); + ASSERT_NEAR(poly->points[0].x, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[0].y, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[1].x, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[1].y, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[2].x, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[2].y, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[3].x, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[3].y, -1.0 + FRAME_SHIFT, EPSILON); + + // Try to transform to incorrect frame + ASSERT_FALSE(polygon_->toFrame("incorrect_frame", tf_buffer_, 0.1)); +} + +//---------- Circles testcases ---------- + +TEST_F(Tester, testCircleObtainParams) +{ + setCircleParams("01010101-0101-0101-0101-010101010101"); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE); + ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(circle_->isFill(), true); + ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-010101010101"); +} + +TEST_F(Tester, testCircleObtainIncorrectParams) +{ + const std::string circle_name(CIRCLE_NAME); + + // Setting circle parameters w/o center and radius + node_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + node_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + node_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + circle_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY); + node_->declare_parameter( + circle_name + ".radius", rclcpp::PARAMETER_DOUBLE); + + // Check that center and radius are mandatory parameter for the circle + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + // Setting incorrect radius + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", -1.0)); + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + // Setting incorrect center format + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", std::vector{0.0})); + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + // Setting incorrect UUID + node_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", "incorrect-uuid")); + ASSERT_FALSE(circle_->obtainParams(circle_name)); +} + +TEST_F(Tester, testCircleSetParams) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2}); + + // Check that parameters were set correctly + ASSERT_TRUE(circle_->setParams(co)); + ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE); + ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(circle_->isFill(), true); + ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-020202020202"); + + // Verify that getting the parameters also works correctly + nav2_msgs::msg::CircleObject::SharedPtr params = circle_->getParams(); + compareCircleObjects(params, co); +} + +TEST_F(Tester, testCircleSetIncorrectParams) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject( + std::vector()); + // Setting incorrect radius + co->radius = -1.0; + + // And check that it is failed to set these parameters + ASSERT_FALSE(circle_->setParams(co)); +} + +TEST_F(Tester, testCircleBoundaries) +{ + setCircleParams(""); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + double min_x, min_y, max_x, max_y; + circle_->getBoundaries(min_x, min_y, max_x, max_y); + + ASSERT_NEAR(min_x, -1.0, EPSILON); + ASSERT_NEAR(min_y, -1.0, EPSILON); + ASSERT_NEAR(max_x, 1.0, EPSILON); + ASSERT_NEAR(max_y, 1.0, EPSILON); +} + +TEST_F(Tester, testCirclePoints) +{ + setCircleParams(""); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + ASSERT_TRUE(circle_->isPointInside(0.0, 0.0)); + ASSERT_FALSE(circle_->isPointInside(-1.0, -1.0)); + ASSERT_FALSE(circle_->isPointInside(1.0, 1.0)); +} + +TEST_F(Tester, testCircleBorders) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyCircleBorders(map); +} + +TEST_F(Tester, testCircleBordersOutOfBoundaries) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + // Set circle to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(CIRCLE_NAME) + ".center", + std::vector{5.0, 5.0})); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testCircleBordersOutsideMap) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + // Set circle to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(CIRCLE_NAME) + ".center", + std::vector{-3.0, -3.0})); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testCircleDifferentFrame) +{ + setCircleParams(""); + // Change shape's frame ID to differ from global one + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".frame_id", SHAPE_FRAME_ID)); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + sendTransform(); + + // Check that shape coordinates are not transformed + geometry_msgs::msg::Point32::SharedPtr center = circle_->getCenter(); + ASSERT_NEAR(center->x, 0.0, EPSILON); + ASSERT_NEAR(center->y, 0.0, EPSILON); + // Transform shape coordinates to global frame + ASSERT_TRUE(circle_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0)); + + // Verify that shape coordinates were transformed to global frame successfully + center = circle_->getCenter(); + ASSERT_NEAR(center->x, FRAME_SHIFT, EPSILON); + ASSERT_NEAR(center->y, FRAME_SHIFT, EPSILON); + + // Try to transform to incorrect frame + ASSERT_FALSE(circle_->toFrame("incorrect_frame", tf_buffer_, 0.1)); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 931af234887..c28c711dc50 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) @@ -59,6 +60,7 @@ target_link_libraries(mppi_controller PUBLIC nav2_ros_common::nav2_ros_common nav2_costmap_2d::layers nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 68db5b73719..630aa19f214 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -59,6 +59,7 @@ This process is then repeated a number of times and returns a converged solution | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | | publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. | + | publish_critics_stats | bool | Default false. Whether to publish statistics about each critic's performance. When enabled, publishes a `nav2_msgs::msg::CriticsStats` message containing critic names, whether they changed costs, and the sum of costs added by each critic. Useful for debugging and tuning critic behavior. | #### Trajectory Visualizer @@ -293,6 +294,7 @@ controller_server: |---------------------------|----------------------------------|-----------------------------------------------------------------------| | `trajectories` | `visualization_msgs/MarkerArray` | Randomly generated trajectories, including resulting control sequence | | `transformed_global_plan` | `nav_msgs/Path` | Part of global plan considered by local planner | +| `critics_stats` | `nav2_msgs/CriticsStats` | Statistics about each critic's performance (published when `publish_critics_stats` is enabled) | ## Notes to Users @@ -328,6 +330,26 @@ As you increase or decrease your weights on the Obstacle, you may notice the afo Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off. +### Critic costs debugging + +The `publish_critics_stats` parameter enables publishing of statistics about each critic's performance, which can be visualized using tools like PlotJuggler or Foxglove to analyze and debug critic behavior. + +The published `nav2_msgs::msg::CriticsStats` message contains the following fields: + +- **stamp**: Timestamp of when the statistics were computed +- **critics**: Array of critic names that were evaluated (e.g., "ConstraintCritic", "GoalCritic", "ObstaclesCritic") +- **changed**: Boolean array indicating whether each critic modified the trajectory costs. `true` means the critic added non-zero costs, `false` means it had no effect +- **costs_sum**: Array of the total cost contribution from each critic. This represents the sum of all costs added by that specific critic across all trajectory candidates + +This data is invaluable for understanding: +- Which critics are actively influencing trajectory selection +- The relative impact of each critic on the final trajectory choice +- Whether critics are working as expected or if parameter tuning is needed (e.g. `threshold_to_consider`) + +More detailed statistics could be added in the future. + +![critics_stats](media/critics_stats.png) + ### MFMA and AVX2 Optimizations This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index fed85867905..237649bedde 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -22,6 +22,7 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav2_msgs/msg/critics_stats.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -96,6 +97,9 @@ class CriticManager std::unique_ptr> loader_; Critics critics_; + nav2::Publisher::SharedPtr critics_effect_pub_; + bool publish_critics_stats_; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/media/critics_stats.png b/nav2_mppi_controller/media/critics_stats.png new file mode 100644 index 00000000000..c373f8e0dcc Binary files /dev/null and b/nav2_mppi_controller/media/critics_stats.png differ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 450dd5254cc..d6301408998 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -13,10 +13,12 @@ angles backward_ros + benchmark geometry_msgs nav2_common nav2_core nav2_costmap_2d + nav2_msgs nav2_util nav_msgs pluginlib diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index d6acd320b05..305140e2e13 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -37,6 +37,7 @@ void CriticManager::getParams() auto node = parent_.lock(); auto getParam = parameters_handler_->getParamGetter(name_); getParam(critic_names_, "critics", std::vector{}, ParameterType::Static); + getParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static); } void CriticManager::loadCritics() @@ -46,6 +47,13 @@ void CriticManager::loadCritics() "nav2_mppi_controller", "mppi::critics::CriticFunction"); } + auto node = parent_.lock(); + if (publish_critics_stats_) { + critics_effect_pub_ = node->create_publisher( + "~/critics_stats"); + critics_effect_pub_->on_activate(); + } + critics_.clear(); for (auto name : critic_names_) { std::string fullname = getFullName(name); @@ -67,11 +75,44 @@ std::string CriticManager::getFullName(const std::string & name) void CriticManager::evalTrajectoriesScores( CriticData & data) const { - for (const auto & critic : critics_) { + std::unique_ptr stats_msg; + if (publish_critics_stats_) { + stats_msg = std::make_unique(); + stats_msg->critics.reserve(critics_.size()); + stats_msg->changed.reserve(critics_.size()); + stats_msg->costs_sum.reserve(critics_.size()); + } + + for (size_t i = 0; i < critics_.size(); ++i) { if (data.fail_flag) { break; } - critic->score(data); + + // Store costs before critic evaluation + Eigen::ArrayXf costs_before; + if (publish_critics_stats_) { + costs_before = data.costs; + } + + critics_[i]->score(data); + + // Calculate statistics if publishing is enabled + if (publish_critics_stats_) { + stats_msg->critics.push_back(critic_names_[i]); + + // Calculate sum of costs added by this individual critic + Eigen::ArrayXf cost_diff = data.costs - costs_before; + float costs_sum = cost_diff.sum(); + stats_msg->costs_sum.push_back(costs_sum); + stats_msg->changed.push_back(costs_sum != 0.0f); + } + } + + // Publish statistics if enabled + if (critics_effect_pub_) { + auto node = parent_.lock(); + stats_msg->stamp = node->get_clock()->now(); + critics_effect_pub_->publish(std::move(stats_msg)); } } diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 7e710fc7375..1228e81d60a 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -50,12 +50,15 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) "~/transformed_global_plan", [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "map", parameters_handler.get()); vis.on_activate(); vis.visualize(pub_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(received_path.poses.size(), 5u); EXPECT_EQ(received_path.header.frame_id, "fake_frame"); } @@ -70,6 +73,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) auto my_sub = node->create_subscription( "~/candidate_trajectories", [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); // optimal_trajectory empty, should fail to publish Eigen::ArrayXXf optimal_trajectory; @@ -81,7 +86,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(received_msg.markers.size(), 0u); // Now populated with content, should publish @@ -89,7 +94,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Should have 20 trajectory points in the map frame EXPECT_EQ(received_msg.markers.size(), 20u); @@ -134,6 +139,9 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) "~/candidate_trajectories", [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + models::Trajectories candidate_trajectories; candidate_trajectories.x = Eigen::ArrayXXf::Ones(200, 12); candidate_trajectories.y = Eigen::ArrayXXf::Ones(200, 12); @@ -146,7 +154,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // 40 * 4, for 5 trajectory steps + 3 point steps EXPECT_EQ(received_msg.markers.size(), 160u); } @@ -165,6 +173,9 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) "~/optimal_path", [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + // optimal_trajectory empty, should fail to publish Eigen::ArrayXXf optimal_trajectory; TrajectoryVisualizer vis; @@ -174,7 +185,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(received_path.poses.size(), 0u); // Now populated with content, should publish @@ -187,7 +198,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); vis.visualize(bogus_path); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Should have a 20 points path in the map frame and with same stamp than velocity command EXPECT_EQ(received_path.poses.size(), 20u); diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp index 2772ea9a858..04e18bb191d 100644 --- a/nav2_mppi_controller/test/utils/utils.hpp +++ b/nav2_mppi_controller/test/utils/utils.hpp @@ -35,9 +35,11 @@ using namespace std::chrono_literals; // NOLINT template void waitSome(const std::chrono::nanoseconds & duration, TNode & node) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); rclcpp::Time start_time = node->now(); while (rclcpp::ok() && node->now() - start_time <= rclcpp::Duration(duration)) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(3ms); } } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 93f7998c56a..41311173486 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) find_package(geographic_msgs) find_package(action_msgs REQUIRED) +find_package(unique_identifier_msgs REQUIRED) nav2_package() @@ -30,6 +31,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VoxelGrid.msg" "msg/BehaviorTreeStatusChange.msg" "msg/BehaviorTreeLog.msg" + "msg/CriticsStats.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/WaypointStatus.msg" @@ -40,6 +42,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Trajectory.msg" "msg/TrajectoryPoint.msg" "srv/GetCosts.srv" + "msg/PolygonObject.msg" + "msg/CircleObject.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" @@ -51,6 +55,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveMap.srv" "srv/SetInitialPose.srv" "srv/ReloadDockDatabase.srv" + "srv/AddShapes.srv" + "srv/RemoveShapes.srv" + "srv/GetShapes.srv" "srv/SetRouteGraph.srv" "srv/DynamicEdges.srv" "action/AssistedTeleop.action" @@ -71,7 +78,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/UndockRobot.action" "action/ComputeRoute.action" "action/ComputeAndTrackRoute.action" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs unique_identifier_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/nav2_msgs/msg/CircleObject.msg b/nav2_msgs/msg/CircleObject.msg new file mode 100644 index 00000000000..2aa64adab62 --- /dev/null +++ b/nav2_msgs/msg/CircleObject.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +unique_identifier_msgs/UUID uuid +geometry_msgs/Point32 center +float32 radius +bool fill +int8 value diff --git a/nav2_msgs/msg/CriticsStats.msg b/nav2_msgs/msg/CriticsStats.msg new file mode 100644 index 00000000000..f02406228df --- /dev/null +++ b/nav2_msgs/msg/CriticsStats.msg @@ -0,0 +1,5 @@ +# Critics statistics message +builtin_interfaces/Time stamp +string[] critics +bool[] changed +float32[] costs_sum diff --git a/nav2_msgs/msg/PolygonObject.msg b/nav2_msgs/msg/PolygonObject.msg new file mode 100644 index 00000000000..b5c9721c629 --- /dev/null +++ b/nav2_msgs/msg/PolygonObject.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +unique_identifier_msgs/UUID uuid +geometry_msgs/Point32[] points +bool closed +int8 value diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 61e84addc37..33de880662e 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -21,6 +21,7 @@ action_msgs nav_msgs geographic_msgs + unique_identifier_msgs rosidl_interface_packages diff --git a/nav2_msgs/srv/AddShapes.srv b/nav2_msgs/srv/AddShapes.srv new file mode 100644 index 00000000000..1b3b5753874 --- /dev/null +++ b/nav2_msgs/srv/AddShapes.srv @@ -0,0 +1,6 @@ +# Add/update vector objects on map + +CircleObject[] circles +PolygonObject[] polygons +--- +bool success diff --git a/nav2_msgs/srv/GetShapes.srv b/nav2_msgs/srv/GetShapes.srv new file mode 100644 index 00000000000..79b7d6c431c --- /dev/null +++ b/nav2_msgs/srv/GetShapes.srv @@ -0,0 +1,5 @@ +# Get vector objects which are now being applied on map + +--- +CircleObject[] circles +PolygonObject[] polygons diff --git a/nav2_msgs/srv/RemoveShapes.srv b/nav2_msgs/srv/RemoveShapes.srv new file mode 100644 index 00000000000..a001f9360ef --- /dev/null +++ b/nav2_msgs/srv/RemoveShapes.srv @@ -0,0 +1,6 @@ +# Remove vector objects from map + +bool all_objects +unique_identifier_msgs/UUID[] uuids +--- +bool success diff --git a/nav2_ros_common/test/test_actions.cpp b/nav2_ros_common/test/test_actions.cpp index 7da319e4e2e..5bda2e7b891 100644 --- a/nav2_ros_common/test/test_actions.cpp +++ b/nav2_ros_common/test/test_actions.cpp @@ -173,8 +173,10 @@ class RclCppFixture { auto node = std::make_shared(); node->on_init(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); while (rclcpp::ok() && !stop_.load()) { - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(10ms); } node->on_term(); diff --git a/nav2_ros_common/test/test_service_client.cpp b/nav2_ros_common/test/test_service_client.cpp index 99d6576292d..ebb5d6705ba 100644 --- a/nav2_ros_common/test/test_service_client.cpp +++ b/nav2_ros_common/test/test_service_client.cpp @@ -85,7 +85,9 @@ TEST(ServiceClient, can_ServiceClient_invoke_in_callback) }); pub->publish(std_msgs::msg::Empty()); - rclcpp::spin_some(sub_node); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); + executor.spin_some(); rclcpp::shutdown(); srv_thread.join(); @@ -98,7 +100,9 @@ TEST(ServiceClient, can_ServiceClient_timeout) rclcpp::init(0, nullptr); auto node = rclcpp::Node::make_shared("test_node"); TestServiceClient t("bar", node, true); - rclcpp::spin_some(node); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin_some(); bool ready = t.wait_for_service(std::chrono::milliseconds(10)); rclcpp::shutdown(); ASSERT_EQ(ready, false); @@ -128,7 +132,9 @@ TEST(ServiceClient, can_ServiceClient_async_call) { // Test async_call client.async_call(req, callback); std::this_thread::sleep_for(std::chrono::seconds(1)); - rclcpp::spin_some(node); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin_some(); rclcpp::shutdown(); srv_thread.join(); diff --git a/nav2_ros_common/test/test_service_server.cpp b/nav2_ros_common/test/test_service_server.cpp index 7365e34afe7..9e42d2a21d1 100644 --- a/nav2_ros_common/test/test_service_server.cpp +++ b/nav2_ros_common/test/test_service_server.cpp @@ -51,7 +51,6 @@ TEST(ServiceServer, can_handle_all_introspection_modes) for (const auto & mode : introspection_modes) { int a = 0; auto node = rclcpp::Node::make_shared("test_node_" + mode); - node->declare_parameter("introspection_mode", mode); auto callback = [&a](const std::shared_ptr, @@ -64,13 +63,17 @@ TEST(ServiceServer, can_handle_all_introspection_modes) auto client_node = rclcpp::Node::make_shared("client_node_" + mode); auto client = client_node->create_client("empty_srv_" + mode); + rclcpp::executors::SingleThreadedExecutor node_executor; + node_executor.add_node(node); + rclcpp::executors::SingleThreadedExecutor client_node_executor; + client_node_executor.add_node(client_node); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); auto req = std::make_shared(); auto result = client->async_send_request(req); - rclcpp::spin_some(node); - rclcpp::spin_some(client_node); + node_executor.spin_some(); + client_node_executor.spin_some(); ASSERT_EQ(a, 1); } } diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp index 84b544a1be5..7255ae33904 100644 --- a/nav2_route/test/test_graph_loader.cpp +++ b/nav2_route/test/test_graph_loader.cpp @@ -74,6 +74,8 @@ TEST(GraphLoader, test_api) TEST(GraphLoader, test_transformation_api) { auto node = std::make_shared("graph_loader_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); auto tf = std::make_shared(node->get_clock()); tf->setUsingDedicatedThread(true); auto tf_listener = std::make_shared(*tf); @@ -107,7 +109,7 @@ TEST(GraphLoader, test_transformation_api) tf_broadcaster->sendTransform(transform); rclcpp::Rate(1).sleep(); tf_broadcaster->sendTransform(transform); - rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + executor.spin_all(std::chrono::milliseconds(50)); graph[0].coords.frame_id = "map_test"; EXPECT_EQ(graph[0].coords.frame_id, "map_test"); diff --git a/nav2_route/test/test_graph_saver.cpp b/nav2_route/test/test_graph_saver.cpp index bd69a9bc112..49c3747ebac 100644 --- a/nav2_route/test/test_graph_saver.cpp +++ b/nav2_route/test/test_graph_saver.cpp @@ -110,6 +110,8 @@ TEST(GraphSaver, test_api) TEST(GraphSaver, test_transformation_api) { auto node = std::make_shared("graph_saver_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); auto tf = std::make_shared(node->get_clock()); tf->setUsingDedicatedThread(true); auto tf_listener = std::make_shared(*tf); @@ -143,7 +145,7 @@ TEST(GraphSaver, test_transformation_api) tf_broadcaster->sendTransform(transform); rclcpp::Rate(1).sleep(); tf_broadcaster->sendTransform(transform); - rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + executor.spin_all(std::chrono::milliseconds(50)); GraphSaver graph_saver(node, tf, frame); std::string file_path = "test.geojson"; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index c96551ae826..fcbc1d1b11a 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -88,6 +88,7 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; // The Node pointer that we need to keep alive for the duration of this plugin. std::shared_ptr node_ptr_; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 44ad28d279e..8c99a270d64 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -112,7 +112,7 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; - + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; // Timeout value when waiting for action servers to respond std::chrono::milliseconds server_timeout_; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp index ba52b009372..86ef9670348 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp @@ -33,10 +33,12 @@ namespace nav2_rviz_plugins * @param server_name The name of the server to load plugins for * @param plugin_type The type of plugin to load * @param combo_box The combo box to add the loaded plugins to + * @param executor The executor to pass to the AsyncParameterClient */ void pluginLoader( rclcpp::Node::SharedPtr node, bool & server_failed, const std::string & server_name, - const std::string & plugin_type, QComboBox * combo_box); + const std::string & plugin_type, QComboBox * combo_box, + rclcpp::Executor::SharedPtr executor = nullptr); // Create label string from goal status msg QString getGoalStatusLabel( diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 2d40b3ae2fd..9f1f33c3e72 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -170,6 +170,8 @@ DockingPanel::DockingPanel(QWidget * parent) undocking_->addTransition(undockingTransition); client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); + executor_ = std::make_shared(); + executor_->add_node(client_node_); state_machine_.addState(pre_initial_); state_machine_.addState(idle_); @@ -252,7 +254,7 @@ DockingPanel::DockingPanel(QWidget * parent) if (!plugins_loaded_) { RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins"); nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); + client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_, executor_); plugins_loaded_ = true; } }); @@ -451,7 +453,7 @@ void DockingPanel::onUndockingButtonPressed() }; auto future_goal_handle = undock_client_->async_send_goal(goal_msg, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + if (executor_->spin_until_future_complete(future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); @@ -490,7 +492,7 @@ void DockingPanel::onCancelDocking() if (dock_goal_handle_) { auto future_cancel = dock_client_->async_cancel_goal(dock_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); @@ -507,7 +509,7 @@ void DockingPanel::onCancelUndocking() if (undock_goal_handle_) { auto future_cancel = undock_client_->async_cancel_goal(undock_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); @@ -530,7 +532,7 @@ void DockingPanel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = dock_goal_handle_->get_status(); // Check if the goal is still executing @@ -549,7 +551,7 @@ void DockingPanel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = undock_goal_handle_->get_status(); // Check if the goal is still executing diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index e6780574604..8da22379965 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -455,6 +455,8 @@ Nav2Panel::Nav2Panel(QWidget * parent) auto options = rclcpp::NodeOptions().arguments( {"--ros-args", "--remap", "__node:=rviz_navigation_dialog_action_client", "--"}); client_node_ = std::make_shared("_", options); + executor_ = std::make_shared(); + executor_->add_node(client_node_); client_nav_ = std::make_shared( "lifecycle_manager_navigation", client_node_); @@ -989,7 +991,7 @@ Nav2Panel::onCancelButtonPressed() if (navigation_goal_handle_) { auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); @@ -1002,7 +1004,7 @@ Nav2Panel::onCancelButtonPressed() auto future_cancel = waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); @@ -1015,7 +1017,7 @@ Nav2Panel::onCancelButtonPressed() auto future_cancel = nav_through_poses_action_client_->async_cancel_goal(nav_through_poses_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + if (executor_->spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel nav through pose action"); @@ -1137,7 +1139,7 @@ Nav2Panel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = waypoint_follower_goal_handle_->get_status(); // Check if the goal is still executing @@ -1158,7 +1160,7 @@ Nav2Panel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = nav_through_poses_goal_handle_->get_status(); // Check if the goal is still executing @@ -1179,7 +1181,7 @@ Nav2Panel::timerEvent(QTimerEvent * event) return; } - rclcpp::spin_some(client_node_); + executor_->spin_some(); auto status = navigation_goal_handle_->get_status(); // Check if the goal is still executing @@ -1236,7 +1238,7 @@ Nav2Panel::startWaypointFollowing(std::vector p auto future_goal_handle = waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + if (executor_->spin_until_future_complete(future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); @@ -1288,7 +1290,7 @@ Nav2Panel::startNavThroughPoses(nav_msgs::msg::Goals poses) auto future_goal_handle = nav_through_poses_action_client_->async_send_goal(nav_through_poses_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + if (executor_->spin_until_future_complete(future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); @@ -1334,7 +1336,7 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) auto future_goal_handle = navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + if (executor_->spin_until_future_complete(future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index ee291c5c5e3..f833c56f32f 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -22,14 +22,14 @@ namespace nav2_rviz_plugins void pluginLoader( rclcpp::Node::SharedPtr node, bool & server_failed, const std::string & server_name, - const std::string & plugin_type, QComboBox * combo_box) + const std::string & plugin_type, QComboBox * combo_box, rclcpp::Executor::SharedPtr executor) { // Do not load the plugins if the combo box is already populated if (combo_box->count() > 0) { return; } - auto parameter_client = std::make_shared(node, server_name); + auto parameter_client = std::make_shared(node, server_name); // Wait for the service to be available before calling it bool server_unavailable = false; @@ -50,7 +50,23 @@ void pluginLoader( return; } auto parameters = parameter_client->get_parameters({plugin_type}); - auto str_arr = parameters[0].as_string_array(); + if (executor) { + if (executor->spin_until_future_complete(parameters) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), + "Failed to get parameter '%s' from server '%s'", + plugin_type.c_str(), server_name.c_str()); + return; + } + } else { + if (rclcpp::spin_until_future_complete(node, parameters) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), + "Failed to get parameter '%s' from server '%s'", + plugin_type.c_str(), server_name.c_str()); + return; + } + } + + auto str_arr = parameters.get()[0].as_string_array(); combo_box->addItem("Default"); for (auto str : str_arr) { combo_box->addItem(QString::fromStdString(str)); diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 98d040bd9cc..b8d12e7941f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -13,7 +13,6 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include #include #include #include diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp index dfedf68072d..b9a3a428d20 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp @@ -83,6 +83,7 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester() ); stamp_ = node_->now(); + executor_.add_node(node_); } AssistedTeleopBehaviorTester::~AssistedTeleopBehaviorTester() @@ -103,7 +104,7 @@ void AssistedTeleopBehaviorTester::activate() RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); sendInitialPose(); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node_); + executor_.spin_some(); } // Wait for lifecycle_manager_navigation to activate behavior_server @@ -146,7 +147,7 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( auto goal_handle_future = client_ptr_->async_send_goal(nav2_msgs::action::AssistedTeleop::Goal()); - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + if (executor_.spin_until_future_complete(goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); @@ -182,7 +183,7 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( return false; } - rclcpp::spin_some(node_); + executor_.spin_some(); r.sleep(); } @@ -190,7 +191,7 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( preempt_pub_->publish(preempt_msg); RCLCPP_INFO(node_->get_logger(), "Waiting for result"); - if (rclcpp::spin_until_future_complete(node_, result_future) != + if (executor_.spin_until_future_complete(result_future) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp index 40e2aa9a754..a35b3a59c6c 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp @@ -79,6 +79,7 @@ class AssistedTeleopBehaviorTester std::shared_ptr tf_listener_; rclcpp::Node::SharedPtr node_; + rclcpp::executors::SingleThreadedExecutor executor_; // Publishers rclcpp::Publisher::SharedPtr initial_pose_pub_; diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp index dffd52f9115..68fb6d9a4a8 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp @@ -68,12 +68,13 @@ void WaitBehaviorTester::activate() throw std::runtime_error("Trying to activate while already active"); return; } - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_); while (!initial_pose_received_) { RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); sendInitialPose(); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node_); + executor.spin_some(); } // Wait for lifecycle_manager_navigation to activate behavior_server diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index b81cef7ce56..01fe9656d5b 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -76,6 +76,7 @@ controller_server: temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" + publish_critics_stats: true visualize: true regenerate_noises: true TrajectoryVisualizer: diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp index 768fd06c449..aabc414a544 100644 --- a/nav2_system_tests/src/localization/test_localization_node.cpp +++ b/nav2_system_tests/src/localization/test_localization_node.cpp @@ -32,10 +32,11 @@ class TestAmclPose : public ::testing::Test tol_ = 0.25; node = rclcpp::Node::make_shared("localization_test"); + executor_.add_node(node); while (node->count_subscribers("scan") < 1) { std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node); + executor_.spin_some(); } initial_pose_pub_ = node->create_publisher( @@ -50,6 +51,7 @@ class TestAmclPose : public ::testing::Test protected: std::shared_ptr node; + rclcpp::executors::SingleThreadedExecutor executor_; void initTestPose(); private: @@ -76,7 +78,7 @@ bool TestAmclPose::defaultAmclTest() // TODO(mhpanah): Initial pose should only be published once. initial_pose_pub_->publish(testPose_); std::this_thread::sleep_for(1s); - rclcpp::spin_some(node); + executor_.spin_some(); } if (std::abs(amcl_pose_x - testPose_.pose.pose.position.x) < tol_ && std::abs(amcl_pose_y - testPose_.pose.pose.position.y) < tol_) diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt index 0a478f2fb9b..09813efc4d7 100644 --- a/nav2_theta_star_planner/CMakeLists.txt +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -87,7 +87,8 @@ ament_export_libraries(${library_name}) ament_export_dependencies( geometry_msgs nav2_core - nav2_costmap_2dnav2_util + nav2_costmap_2d + nav2_util nav_msgs rclcpp rclcpp_lifecycle diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 3a56cb5feea..d67f17e3c7c 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -188,6 +188,49 @@ inline int find_next_matching_goal_in_waypoint_statuses( return itr - waypoint_statuses.begin(); } +/** + * @brief Checks if point is inside the polygon + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @param polygon Polygon to check if the point is inside + * @return True if given point is inside polygon, otherwise false + */ +template +inline bool isPointInsidePolygon( + const double px, const double py, const std::vector & polygon) +{ + // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." + // Communications of the ACM 5.8 (1962): 434. + // Implementation of ray crossings algorithm for point in polygon task solving. + // Y coordinate is fixed. Moving the ray on X+ axis starting from given point. + // Odd number of intersections with polygon boundaries means the point is inside polygon. + const int points_num = polygon.size(); + int i, j; // Polygon vertex iterators + bool res = false; // Final result, initialized with already inverted value + + // Starting from the edge where the last point of polygon is connected to the first + i = points_num - 1; + for (j = 0; j < points_num; j++) { + // Checking the edge only if given point is between edge boundaries by Y coordinates. + // One of the condition should contain equality in order to exclude the edges + // parallel to X+ ray. + if ((py <= polygon[i].y) == (py > polygon[j].y)) { + // Calculating the intersection coordinate of X+ ray + const double x_inter = polygon[i].x + + (py - polygon[i].y) * + (polygon[j].x - polygon[i].x) / + (polygon[j].y - polygon[i].y); + // If intersection with checked edge is greater than point x coordinate, + // inverting the result + if (x_inter > px) { + res = !res; + } + } + i = j; + } + return res; +} + } // namespace geometry_utils } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/occ_grid_utils.hpp b/nav2_util/include/nav2_util/occ_grid_utils.hpp new file mode 100644 index 00000000000..26bb8740f2b --- /dev/null +++ b/nav2_util/include/nav2_util/occ_grid_utils.hpp @@ -0,0 +1,100 @@ +// Copyright (c) 2008, 2013, Willow Garage, Inc. +// Copyright (c) 2023 Samsung R&D Institute Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Eitan Marder-Eppstein +// David V. Lu!! +// Alexey Merzlyakov + +#ifndef NAV2_UTIL__OCC_GRID_UTILS_HPP_ +#define NAV2_UTIL__OCC_GRID_UTILS_HPP_ + +#include "nav_msgs/msg/occupancy_grid.hpp" + +namespace nav2_util +{ + +/** + * @brief: Convert from world coordinates to map coordinates. + Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. + * @param map OccupancyGrid map on which to convert + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ +inline bool worldToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + const double wx, const double wy, unsigned int & mx, unsigned int & my) +{ + const double origin_x = map->info.origin.position.x; + const double origin_y = map->info.origin.position.y; + const double resolution = map->info.resolution; + const unsigned int size_x = map->info.width; + const unsigned int size_y = map->info.height; + + if (wx < origin_x || wy < origin_y) { + return false; + } + + mx = static_cast((wx - origin_x) / resolution); + my = static_cast((wy - origin_y) / resolution); + if (mx >= size_x || my >= size_y) { + return false; + } + + return true; +} + +/** + * @brief Convert from map coordinates to world coordinates + * @param mx The x map coordinate + * @param my The y map coordinate + * @param wx Will be set to the associated world x coordinate + * @param wy Will be set to the associated world y coordinate + */ +inline void mapToWorld( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + const unsigned int mx, const unsigned int my, double & wx, double & wy) +{ + const double origin_x = map->info.origin.position.x; + const double origin_y = map->info.origin.position.y; + const double resolution = map->info.resolution; + + wx = origin_x + (mx + 0.5) * resolution; + wy = origin_y + (my + 0.5) * resolution; +} + +} // namespace nav2_util + +#endif // NAV2_UTIL__OCC_GRID_UTILS_HPP_ diff --git a/nav2_util/include/nav2_util/raytrace_line_2d.hpp b/nav2_util/include/nav2_util/raytrace_line_2d.hpp new file mode 100644 index 00000000000..b44d1e79a8d --- /dev/null +++ b/nav2_util/include/nav2_util/raytrace_line_2d.hpp @@ -0,0 +1,146 @@ +// Copyright (c) 2008, 2013, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Eitan Marder-Eppstein +// David V. Lu!! + +#ifndef NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ +#define NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ + +#include +#include +#include + +namespace nav2_util +{ + +/** + * @brief get the sign of an int + */ +inline int sign(int x) +{ + return x > 0 ? 1 : -1; +} + +/** + * @brief A 2D implementation of Bresenham's raytracing algorithm... + * applies an action at each step + */ +template +inline void bresenham2D( + ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, + int offset_a, int offset_b, unsigned int offset, + unsigned int max_length) +{ + unsigned int end = std::min(max_length, abs_da); + for (unsigned int i = 0; i < end; ++i) { + at(offset); + offset += offset_a; + error_b += abs_db; + if ((unsigned int)error_b >= abs_da) { + offset += offset_b; + error_b -= abs_da; + } + } + at(offset); +} + +/** + * @brief Raytrace a line and apply some action at each step + * @param at The action to take... a functor + * @param x0 The starting x coordinate + * @param y0 The starting y coordinate + * @param x1 The ending x coordinate + * @param y1 The ending y coordinate + * @param step_x OX-step on map + * @param max_length The maximum desired length of the segment... + * allows you to not go all the way to the endpoint + * @param min_length The minimum desired length of the segment + */ +template +inline void raytraceLine( + ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, unsigned int step_x, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) +{ + int dx_full = x1 - x0; + int dy_full = y1 - y0; + + // we need to chose how much to scale our dominant dimension, + // based on the maximum length of the line + double dist = std::hypot(dx_full, dy_full); + if (dist < min_length) { + return; + } + + unsigned int min_x0, min_y0; + if (dist > 0.0) { + // Adjust starting point and offset to start from min_length distance + min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); + min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); + } else { + // dist can be 0 if [x0, y0]==[x1, y1]. + // In this case only this cell should be processed. + min_x0 = x0; + min_y0 = y0; + } + unsigned int offset = min_y0 * step_x + min_x0; + + int dx = x1 - min_x0; + int dy = y1 - min_y0; + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + + int offset_dx = sign(dx); + int offset_dy = sign(dy) * step_x; + + double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); + // if x is dominant + if (abs_dx >= abs_dy) { + int error_y = abs_dx / 2; + + bresenham2D( + at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); + return; + } + + // otherwise y is dominant + int error_x = abs_dy / 2; + + bresenham2D( + at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); +} + +} // namespace nav2_util + +#endif // NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 09d7d4a38c0..c8447eaab6e 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -1,3 +1,5 @@ +add_subdirectory(regression) + ament_add_gtest(test_execution_timer test_execution_timer.cpp) target_link_libraries(test_execution_timer ${library_name}) diff --git a/nav2_util/test/regression/CMakeLists.txt b/nav2_util/test/regression/CMakeLists.txt new file mode 100644 index 00000000000..f216fbe7751 --- /dev/null +++ b/nav2_util/test/regression/CMakeLists.txt @@ -0,0 +1,3 @@ +# Bresenham2D corner cases test +ament_add_gtest(map_bresenham_2d map_bresenham_2d.cpp) +target_link_libraries(map_bresenham_2d ${library_name}) diff --git a/nav2_util/test/regression/map_bresenham_2d.cpp b/nav2_util/test/regression/map_bresenham_2d.cpp new file mode 100644 index 00000000000..220962948e5 --- /dev/null +++ b/nav2_util/test/regression/map_bresenham_2d.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Alexey Merzlyakov + +#include + +#include + +#include "nav2_util/raytrace_line_2d.hpp" + +// MapAction - is a functor class used to cover raytraceLine algorithm. +// It contains char map inside, which is an abstract one and not related +// to any concrete representation (like Costmap2D or OccupancyGrid). +class MapAction +{ +public: + explicit MapAction( + char * map, unsigned int size, char mark_val = 100) + : map_(map), size_(size), mark_val_(mark_val) + { + } + + inline void operator()(unsigned int off) + { + ASSERT_TRUE(off < size_); + map_[off] = mark_val_; + } + + inline unsigned int get(unsigned int off) + { + return map_[off]; + } + +private: + char * map_; + unsigned int size_; + char mark_val_; +}; + +class MapTest +{ +public: + MapTest( + unsigned int size_x, unsigned int size_y, + char default_val = 0) + : size_x_(size_x), size_y_(size_y) + { + map_ = new char[size_x * size_y]; + memset(map_, default_val, size_x * size_y); + } + + char * getMap() + { + return map_; + } + + unsigned int getSize() + { + return size_x_ * size_y_; + } + + void raytraceLine( + MapAction ma, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) + { + nav2_util::raytraceLine(ma, x0, y0, x1, y1, size_x_, max_length, min_length); + } + +private: + char * map_; + unsigned int size_x_, size_y_; +}; + +TEST(map_2d, bresenham2DBoundariesCheck) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 6; + MapTest mt(sz_x, sz_y); + MapAction ma(mt.getMap(), mt.getSize()); + + // Initial point - some asymmetrically standing point in order to cover most corner cases + const unsigned int x0 = 2; + const unsigned int y0 = 4; + // (x1, y1) point will move + unsigned int x1, y1; + + // Running on (x, 0) edge + y1 = 0; + for (x1 = 0; x1 < sz_x; x1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (x, sz_y) edge + y1 = sz_y - 1; + for (x1 = 0; x1 < sz_x; x1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (0, y) edge + x1 = 0; + for (y1 = 0; y1 < sz_y; y1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (sz_x, y) edge + x1 = sz_x - 1; + for (y1 = 0; y1 < sz_y; y1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } +} + +TEST(map_2d, bresenham2DSamePoint) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 0; + MapTest mt(sz_x, sz_y, 0.1); + MapAction ma(mt.getMap(), mt.getSize()); + + // Initial point + const double x0 = 2; + const double y0 = 4; + + unsigned int offset = y0 * sz_x + x0; + char val_before = ma.get(offset); + // Same point to check + mt.raytraceLine(ma, x0, y0, x0, y0, max_length, min_length); + char val_after = ma.get(offset); + ASSERT_FALSE(val_before == val_after); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp index a3cdda664f0..641e13ec98f 100644 --- a/nav2_util/test/test_base_footprint_publisher.cpp +++ b/nav2_util/test/test_base_footprint_publisher.cpp @@ -22,7 +22,9 @@ TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) { auto node = std::make_shared(); - rclcpp::spin_some(node->get_node_base_interface()); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin_some(); auto tf_broadcaster = std::make_shared(node); auto buffer = std::make_shared(node->get_clock()); @@ -41,7 +43,7 @@ TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) transform.header.frame_id = "test1_1"; transform.child_frame_id = "test1"; tf_broadcaster->sendTransform(transform); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); EXPECT_THROW( buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero), tf2::TransformException); @@ -56,7 +58,7 @@ TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) tf_broadcaster->sendTransform(transform); rclcpp::Rate r(1.0); r.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); auto t = buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero); EXPECT_EQ(t.transform.translation.x, 1.0); EXPECT_EQ(t.transform.translation.y, 1.0); diff --git a/nav2_util/test/test_odometry_utils.cpp b/nav2_util/test/test_odometry_utils.cpp index 5b8087e097e..91f9f4a5756 100644 --- a/nav2_util/test/test_odometry_utils.cpp +++ b/nav2_util/test/test_odometry_utils.cpp @@ -66,7 +66,8 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->on_activate(); nav2_util::OdomSmoother odom_smoother(node, 0.3, "odom"); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); nav_msgs::msg::Odometry odom_msg; geometry_msgs::msg::Twist twist_msg; geometry_msgs::msg::Twist twist_raw_msg; @@ -79,7 +80,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_msg.twist.twist.angular.z = 1.0; odom_pub->publish(odom_msg); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); EXPECT_EQ(twist_msg.linear.x, 1.0); @@ -93,7 +94,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->publish(odom_msg); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); twist_raw_msg = odom_smoother.getRawTwist(); @@ -111,7 +112,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->publish(odom_msg); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); twist_raw_msg = odom_smoother.getRawTwist(); @@ -129,7 +130,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->publish(odom_msg); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); twist_raw_msg = odom_smoother.getRawTwist(); @@ -147,7 +148,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->publish(odom_msg); std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); twist_msg = odom_smoother.getTwist(); twist_raw_msg = odom_smoother.getRawTwist(); diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index dc3b307984f..0b716d20623 100644 --- a/nav2_util/test/test_twist_publisher.cpp +++ b/nav2_util/test/test_twist_publisher.cpp @@ -47,9 +47,10 @@ TEST(TwistPublisher, Unstamped) auto my_sub = sub_node->create_subscription( "cmd_vel", [&](const geometry_msgs::msg::Twist msg) {sub_msg = msg;}); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); vel_publisher->publish(std::move(pub_msg)); - rclcpp::spin_some(sub_node->get_node_base_interface()); + executor.spin_some(); EXPECT_EQ(pub_msg_copy.linear.x, sub_msg.linear.x); EXPECT_EQ(vel_publisher->get_subscription_count(), 1); @@ -86,9 +87,10 @@ TEST(TwistPublisher, Stamped) auto my_sub = sub_node->create_subscription( "cmd_vel", [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); vel_publisher->publish(std::move(pub_msg)); - rclcpp::spin_some(sub_node->get_node_base_interface()); + executor.spin_some(); ASSERT_EQ(vel_publisher->get_subscription_count(), 1); EXPECT_EQ(pub_msg_copy, sub_msg); pub_node->deactivate(); diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp index 8e30b6eec21..f4043f899d9 100644 --- a/nav2_util/test/test_twist_subscriber.cpp +++ b/nav2_util/test/test_twist_subscriber.cpp @@ -47,12 +47,13 @@ TEST(TwistSubscriber, Unstamped) auto vel_pub = pub_node->create_publisher("cmd_vel"); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); pub_node->activate(); vel_pub->on_activate(); vel_pub->publish(pub_msg.twist); - rclcpp::spin_some(sub_node->get_node_base_interface()); + executor.spin_some(); ASSERT_EQ(vel_pub->get_subscription_count(), 1); EXPECT_EQ(pub_msg, sub_msg); @@ -87,9 +88,10 @@ TEST(TwistSubscriber, Stamped) pub_node->activate(); vel_pub->on_activate(); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(sub_node->get_node_base_interface()); vel_pub->publish(pub_msg); - rclcpp::spin_some(sub_node->get_node_base_interface()); + executor.spin_some(); ASSERT_EQ(vel_pub->get_subscription_count(), 1); EXPECT_EQ(pub_msg, sub_msg); diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index ae3f36aa1b7..8de8bd7e9a7 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -55,6 +55,8 @@ TEST(VelocitySmootherTest, openLoopTestTimer6dof) { auto smoother = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(smoother->get_node_base_interface()); std::vector deadbands{0.2, 0.0, 0.0, 0.0, 0.0, 0.0}; std::vector min_velocity{-0.5, -0.5, -0.5, -2.5, -2.5, -2.5}; std::vector max_velocity{0.5, 0.5, 0.5, 2.5, 2.5, 2.5}; @@ -96,7 +98,7 @@ TEST(VelocitySmootherTest, openLoopTestTimer6dof) // Process velocity smoothing and send updated odometry based on commands auto start = smoother->now(); while (smoother->now() - start < 1.5s) { - rclcpp::spin_some(smoother->get_node_base_interface()); + executor.spin_some(); } // Sanity check we have the approximately right number of messages for the timespan and timeout @@ -125,6 +127,8 @@ TEST(VelocitySmootherTest, openLoopTestTimer) { auto smoother = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(smoother->get_node_base_interface()); std::vector deadbands{0.2, 0.0, 0.0}; smoother->declare_parameter("scale_velocities", rclcpp::ParameterValue(true)); smoother->set_parameter(rclcpp::Parameter("scale_velocities", true)); @@ -152,7 +156,7 @@ TEST(VelocitySmootherTest, openLoopTestTimer) // Process velocity smoothing and send updated odometry based on commands auto start = smoother->now(); while (smoother->now() - start < 1.5s) { - rclcpp::spin_some(smoother->get_node_base_interface()); + executor.spin_some(); } // Sanity check we have the approximately right number of messages for the timespan and timeout @@ -181,6 +185,8 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer) { auto smoother = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(smoother->get_node_base_interface()); smoother->declare_parameter("feedback", rclcpp::ParameterValue(std::string("CLOSED_LOOP"))); smoother->set_parameter(rclcpp::Parameter("feedback", std::string("CLOSED_LOOP"))); rclcpp_lifecycle::State state; @@ -222,7 +228,7 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer) odom_msg.twist.twist.linear.x = linear_vels.back(); } odom_pub->publish(odom_msg); - rclcpp::spin_some(smoother->get_node_base_interface()); + executor.spin_some(); } // Sanity check we have the approximately right number of messages for the timespan and timeout @@ -633,10 +639,12 @@ TEST(VelocitySmootherTest, testCommandCallback) auto pub = smoother->create_publisher("cmd_vel"); pub->on_activate(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(smoother->get_node_base_interface()); auto msg = std::make_unique(); msg->twist.linear.x = 100.0; pub->publish(std::move(msg)); - rclcpp::spin_some(smoother->get_node_base_interface()); + executor.spin_some(); EXPECT_TRUE(smoother->hasCommandMsg()); EXPECT_EQ(smoother->lastCommandMsg()->twist.linear.x, 100.0); diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp index 726851be5d4..becf874d4c1 100644 --- a/nav2_waypoint_follower/test/test_task_executors.cpp +++ b/nav2_waypoint_follower/test/test_task_executors.cpp @@ -61,19 +61,21 @@ TEST(WaypointFollowerTest, InputAtWaypoint) auto node = std::make_shared("testWaypointNode"); auto pub = node->create_publisher("input_at_waypoint/input"); pub->on_activate(); + rclcpp::executors::SingleThreadedExecutor executor; auto publish_message = [&]() -> void { rclcpp::Rate(5).sleep(); auto msg = std::make_unique(); pub->publish(std::move(msg)); - rclcpp::spin_some(node->shared_from_this()->get_node_base_interface()); + executor.spin_some(); }; std::unique_ptr iaw( new nav2_waypoint_follower::InputAtWaypoint ); iaw->initialize(node, std::string("IAW")); + executor.add_node(node->shared_from_this()->get_node_base_interface()); auto start_time = node->now(); @@ -103,6 +105,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) auto node = std::make_shared("testWaypointNode"); auto pub = node->create_publisher("/camera/color/image_raw"); pub->on_activate(); + rclcpp::executors::SingleThreadedExecutor executor; std::condition_variable cv; std::mutex mtx; std::unique_lock lck(mtx, std::defer_lock); @@ -124,7 +127,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) msg->data.push_back(fake_data++); } pub->publish(std::move(msg)); - rclcpp::spin_some(node->shared_from_this()->get_node_base_interface()); + executor.spin_some(); lck.lock(); data_published = true; cv.notify_one(); @@ -135,6 +138,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) new nav2_waypoint_follower::PhotoAtWaypoint ); paw->initialize(node, std::string("PAW")); + executor.add_node(node->shared_from_this()->get_node_base_interface()); // no images, throws because can't write geometry_msgs::msg::PoseStamped pose;