diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index f0d34789c0e..16f08493ab3 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -45,7 +45,7 @@ Once a new node is registered with the factory, it is now available to the Behav - + diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp index c8c55309b47..1ae7d394929 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp @@ -32,7 +32,7 @@ namespace nav2_behavior_tree /** * @brief The GoalCheckerSelector behavior is used to switch the goal checker * of the controller server. It subscribes to a topic "goal_checker_selector" - * to get the decision about what goal_checker must be used. It is usually used before of + * to get the decision about what goal checker must be used. It is usually used before of * the FollowPath. The selected_goal_checker output port is passed to goal_checker_id * input port of the FollowPath * @note It will re-initialize when halted. @@ -59,16 +59,16 @@ class GoalCheckerSelector : public BT::SyncActionNode return { BT::InputPort( "default_goal_checker", - "the default goal_checker to use if there is not any external topic message received."), + "the default goal checker to use if there is not any external topic message received."), BT::InputPort( "topic_name", "goal_checker_selector", - "the input topic name to select the goal_checker"), + "the input topic name to select the goal checker"), BT::OutputPort( "selected_goal_checker", - "Selected goal_checker by subscription") + "Selected goal checker by subscription") }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp index 09b6a82e5ba..285b9978e1a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp @@ -31,7 +31,7 @@ namespace nav2_behavior_tree /** * @brief The ProgressCheckerSelector behavior is used to switch the progress checker * of the controller server. It subscribes to a topic "progress_checker_selector" - * to get the decision about what progress_checker must be used. It is usually used before of + * to get the decision about what progress checker must be used. It is usually used before of * the FollowPath. The selected_progress_checker output port is passed to progress_checker_id * input port of the FollowPath * @note It will re-initialize when halted. @@ -58,16 +58,16 @@ class ProgressCheckerSelector : public BT::SyncActionNode return { BT::InputPort( "default_progress_checker", - "the default progress_checker to use if there is not any external topic message received."), + "the default progress checker to use if there is not any external topic message received."), BT::InputPort( "topic_name", "progress_checker_selector", - "the input topic name to select the progress_checker"), + "the input topic name to select the progress checker"), BT::OutputPort( "selected_progress_checker", - "Selected progress_checker by subscription") + "Selected progress checker by subscription") }; } diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index fdb1520d9ef..996478e1a95 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -170,7 +170,7 @@ - + Path to be smoothed Maximum smoothing duration Bool if collision check should be performed @@ -182,7 +182,7 @@ - + Path to follow Goal checker Progress checker diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index e9b98261781..0cb42ea75a9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -130,7 +130,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) R"( - + )"; @@ -149,10 +149,10 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("grid_based")); EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, 1.0); EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); - EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("grid_based")); // check if returned path is correct nav_msgs::msg::Path path; @@ -189,7 +189,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) R"( - + )"; @@ -214,11 +214,11 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("grid_based")); EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, 1.0); EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); - EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("grid_based")); // check if returned path is correct nav_msgs::msg::Path path; diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index 3d022205ee0..2c731deafe3 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -127,7 +127,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) R"( - + )"; @@ -146,10 +146,10 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("grid_based")); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0); EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); - EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("grid_based")); // check if returned path is correct nav_msgs::msg::Path path; @@ -186,7 +186,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) R"( - + )"; @@ -211,11 +211,11 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("grid_based")); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0); EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); - EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("grid_based")); // check if returned path is correct nav_msgs::msg::Path path; 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 1362d024b2d..62f19d204b5 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 @@ -100,7 +100,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) R"( - + )"; @@ -115,11 +115,11 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) std::string selected_controller_result; EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); - EXPECT_EQ(selected_controller_result, "DWB"); + EXPECT_EQ(selected_controller_result, "dwb"); std_msgs::msg::String selected_controller_cmd; - selected_controller_cmd.data = "DWC"; + selected_controller_cmd.data = "dwc"; auto controller_selector_pub = node_->create_publisher( @@ -137,7 +137,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) // check controller updated EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); - EXPECT_EQ("DWC", selected_controller_result); + EXPECT_EQ("dwc", selected_controller_result); } TEST_F(ControllerSelectorTestFixture, test_default_topic) @@ -147,7 +147,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) R"( - + )"; @@ -162,11 +162,11 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) std::string selected_controller_result; EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); - EXPECT_EQ(selected_controller_result, "GridBased"); + EXPECT_EQ(selected_controller_result, "follow_path"); std_msgs::msg::String selected_controller_cmd; - selected_controller_cmd.data = "RRT"; + selected_controller_cmd.data = "new_follow_path"; auto controller_selector_pub = node_->create_publisher( @@ -184,7 +184,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) // check controller updated EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); - EXPECT_EQ("RRT", selected_controller_result); + EXPECT_EQ("new_follow_path", selected_controller_result); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 0ac8837b38e..3a1a64ce46a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -122,7 +122,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) R"( - + )"; @@ -141,10 +141,10 @@ TEST_F(FollowPathActionTestFixture, test_tick) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(tree_->rootNode()->getInput("controller_id"), std::string("FollowPath")); + EXPECT_EQ(tree_->rootNode()->getInput("controller_id"), std::string("follow_path")); EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, 1.0); - EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); + EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("follow_path")); // halt node so another goal can be sent tree_->haltTree(); @@ -186,7 +186,7 @@ TEST(FollowPathAction, testProgressCheckerIdUpdate) R"( - + )"; @@ -226,7 +226,7 @@ TEST(FollowPathAction, testGoalCheckerIdUpdate) R"( - + )"; @@ -266,7 +266,7 @@ TEST(FollowPathAction, testControllerIdUpdate) R"( - + )"; 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 c0d0ee35daa..29e940a0dae 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 @@ -100,7 +100,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) R"( - + )"; @@ -115,11 +115,11 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) std::string selected_goal_checker_result; EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); - EXPECT_EQ(selected_goal_checker_result, "SimpleGoalCheck"); + EXPECT_EQ(selected_goal_checker_result, "simple_goal_checker"); std_msgs::msg::String selected_goal_checker_cmd; - selected_goal_checker_cmd.data = "AngularGoalChecker"; + selected_goal_checker_cmd.data = "angular_goal_checker"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -138,7 +138,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) // check goal_checker updated EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); - EXPECT_EQ("AngularGoalChecker", selected_goal_checker_result); + EXPECT_EQ("angular_goal_checker", selected_goal_checker_result); } TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) @@ -148,7 +148,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) R"( - + )"; @@ -163,11 +163,11 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) std::string selected_goal_checker_result; EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); - EXPECT_EQ(selected_goal_checker_result, "GridBased"); + EXPECT_EQ(selected_goal_checker_result, "goal_checker"); std_msgs::msg::String selected_goal_checker_cmd; - selected_goal_checker_cmd.data = "RRT"; + selected_goal_checker_cmd.data = "new_goal_checker"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -186,7 +186,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) // check goal_checker updated EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); - EXPECT_EQ("RRT", selected_goal_checker_result); + EXPECT_EQ("new_goal_checker", selected_goal_checker_result); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/action/test_path_handler_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_path_handler_selector_node.cpp index 8e60a2fc8d2..a4a188fa501 100644 --- a/nav2_behavior_tree/test/plugins/action/test_path_handler_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_path_handler_selector_node.cpp @@ -99,7 +99,7 @@ TEST_F(PathHandlerSelectorTestFixture, test_custom_topic) R"( - + )"; @@ -114,11 +114,11 @@ TEST_F(PathHandlerSelectorTestFixture, test_custom_topic) std::string selected_path_handler_result; EXPECT_TRUE(config_->blackboard->get("selected_path_handler", selected_path_handler_result)); - EXPECT_EQ(selected_path_handler_result, "FeasiblePathHandler"); + EXPECT_EQ(selected_path_handler_result, "feasible_path_handler"); std_msgs::msg::String selected_path_handler_cmd; - selected_path_handler_cmd.data = "HardPathHandler"; + selected_path_handler_cmd.data = "hard_path_handler"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -137,7 +137,7 @@ TEST_F(PathHandlerSelectorTestFixture, test_custom_topic) // check path_handler updated EXPECT_TRUE(config_->blackboard->get("selected_path_handler", selected_path_handler_result)); - EXPECT_EQ("HardPathHandler", selected_path_handler_result); + EXPECT_EQ("hard_path_handler", selected_path_handler_result); } TEST_F(PathHandlerSelectorTestFixture, test_default_topic) @@ -147,7 +147,7 @@ TEST_F(PathHandlerSelectorTestFixture, test_default_topic) R"( - + )"; @@ -162,11 +162,11 @@ TEST_F(PathHandlerSelectorTestFixture, test_default_topic) std::string selected_path_handler_result; EXPECT_TRUE(config_->blackboard->get("selected_path_handler", selected_path_handler_result)); - EXPECT_EQ(selected_path_handler_result, "GridBased"); + EXPECT_EQ(selected_path_handler_result, "path_handler"); std_msgs::msg::String selected_path_handler_cmd; - selected_path_handler_cmd.data = "RRT"; + selected_path_handler_cmd.data = "new_path_handler"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -185,7 +185,7 @@ TEST_F(PathHandlerSelectorTestFixture, test_default_topic) // check path_handler updated EXPECT_TRUE(config_->blackboard->get("selected_path_handler", selected_path_handler_result)); - EXPECT_EQ("RRT", selected_path_handler_result); + EXPECT_EQ("new_path_handler", selected_path_handler_result); } int main(int argc, char ** argv) 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 82ea0635f27..8d246190589 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 @@ -98,7 +98,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) R"( - + )"; @@ -113,11 +113,11 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) std::string selected_planner_result; EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); - EXPECT_EQ(selected_planner_result, "GridBased"); + EXPECT_EQ(selected_planner_result, "grid_based"); std_msgs::msg::String selected_planner_cmd; - selected_planner_cmd.data = "RRT"; + selected_planner_cmd.data = "rrt"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -136,7 +136,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) // check planner updated EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); - EXPECT_EQ("RRT", selected_planner_result); + EXPECT_EQ("rrt", selected_planner_result); } TEST_F(PlannerSelectorTestFixture, test_default_topic) @@ -146,7 +146,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) R"( - + )"; @@ -161,11 +161,11 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) std::string selected_planner_result; EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); - EXPECT_EQ(selected_planner_result, "GridBased"); + EXPECT_EQ(selected_planner_result, "grid_based"); std_msgs::msg::String selected_planner_cmd; - selected_planner_cmd.data = "RRT"; + selected_planner_cmd.data = "rrt"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -184,7 +184,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) // check planner updated EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); - EXPECT_EQ("RRT", selected_planner_result); + EXPECT_EQ("rrt", selected_planner_result); } int main(int argc, char ** argv) 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 c305471f412..47ba4e06694 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 @@ -99,7 +99,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) R"( - + )"; @@ -116,11 +116,11 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) "selected_progress_checker", selected_progress_checker_result); - EXPECT_EQ(selected_progress_checker_result, "SimpleProgressCheck"); + EXPECT_EQ(selected_progress_checker_result, "simple_progress_checker"); std_msgs::msg::String selected_progress_checker_cmd; - selected_progress_checker_cmd.data = "AngularProgressChecker"; + selected_progress_checker_cmd.data = "angular_progress_checker"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -140,7 +140,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) // check progress_checker updated res = config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); - EXPECT_EQ("AngularProgressChecker", selected_progress_checker_result); + EXPECT_EQ("angular_progress_checker", selected_progress_checker_result); } TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) @@ -150,7 +150,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) R"( - + )"; @@ -167,11 +167,11 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) "selected_progress_checker", selected_progress_checker_result); - EXPECT_EQ(selected_progress_checker_result, "GridBased"); + EXPECT_EQ(selected_progress_checker_result, "progress_checker"); std_msgs::msg::String selected_progress_checker_cmd; - selected_progress_checker_cmd.data = "RRT"; + selected_progress_checker_cmd.data = "new_progress_checker"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -190,7 +190,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) // check goal_checker updated res = config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); - EXPECT_EQ("RRT", selected_progress_checker_result); + EXPECT_EQ("new_progress_checker", selected_progress_checker_result); } int main(int argc, char ** argv) 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 e099477de84..237cd038a14 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 @@ -100,7 +100,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) R"( - + )"; @@ -115,11 +115,11 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) std::string selected_smoother_result; EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); - EXPECT_EQ(selected_smoother_result, "DWB"); + EXPECT_EQ(selected_smoother_result, "smoother"); std_msgs::msg::String selected_smoother_cmd; - selected_smoother_cmd.data = "DWC"; + selected_smoother_cmd.data = "new_smoother"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -138,7 +138,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) // check smoother updated EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); - EXPECT_EQ("DWC", selected_smoother_result); + EXPECT_EQ("new_smoother", selected_smoother_result); } TEST_F(SmootherSelectorTestFixture, test_default_topic) @@ -148,7 +148,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) R"( - + )"; @@ -163,11 +163,11 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) std::string selected_smoother_result; EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); - EXPECT_EQ(selected_smoother_result, "GridBased"); + EXPECT_EQ(selected_smoother_result, "smoother"); std_msgs::msg::String selected_smoother_cmd; - selected_smoother_cmd.data = "RRT"; + selected_smoother_cmd.data = "new_smoother"; rclcpp::QoS qos = nav2::qos::LatchedPublisherQoS(); @@ -186,7 +186,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) // check smoother updated EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); - EXPECT_EQ("RRT", selected_smoother_result); + EXPECT_EQ("new_smoother", selected_smoother_result); } int main(int argc, char ** argv) diff --git a/nav2_behaviors/test/test_behaviors.cpp b/nav2_behaviors/test/test_behaviors.cpp index 8a67cf794ff..982382ef1a2 100644 --- a/nav2_behaviors/test/test_behaviors.cpp +++ b/nav2_behaviors/test/test_behaviors.cpp @@ -157,7 +157,7 @@ class BehaviorTest : public ::testing::Test behavior_ = std::make_shared(); behavior_->configure( node_lifecycle_, - "Behavior", + "behavior", tf_buffer_, local_collision_checker_, global_collision_checker_); @@ -167,7 +167,7 @@ class BehaviorTest : public ::testing::Test node_lifecycle_->get_node_base_interface(), node_lifecycle_->get_node_graph_interface(), node_lifecycle_->get_node_logging_interface(), - node_lifecycle_->get_node_waitables_interface(), "Behavior"); + node_lifecycle_->get_node_waitables_interface(), "behavior"); std::cout << "Setup complete." << std::endl; } diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 269d7b34882..d9170d863f9 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -98,8 +98,8 @@ controller_server: failure_tolerance: 0.3 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: ["FollowPath"] - path_handler_plugins: ["PathHandler"] + controller_plugins: ["follow_path"] + path_handler_plugins: ["path_handler"] use_realtime_priority: false speed_limit_topic: "speed_limit" @@ -120,7 +120,7 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 path_length_tolerance: 1.0 - PathHandler: + path_handler: plugin: "nav2_controller::FeasiblePathHandler" prune_distance: 2.0 enforce_path_inversion: false @@ -129,7 +129,7 @@ controller_server: inversion_yaw_tolerance: 0.4 minimum_rotation_angle: 0.785 reject_unit_path: false - FollowPath: + follow_path: plugin: "nav2_mppi_controller::MPPIController" time_steps: 56 model_dt: 0.05 @@ -386,10 +386,10 @@ planner_server: ros__parameters: allow_partial_planning: false expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] + planner_plugins: ["grid_based"] costmap_update_timeout: 1.0 introspection_mode: "disabled" - GridBased: + grid_based: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false @@ -469,18 +469,18 @@ route_server: boundary_radius_to_achieve_node: 1.0 radius_to_achieve_node: 2.0 smooth_corners: true - operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] - ReroutingService: + operations: ["adjust_speed_limit", "rerouting_service", "collision_monitor"] + rerouting_service: plugin: "nav2_route::ReroutingService" - AdjustSpeedLimit: + adjust_speed_limit: plugin: "nav2_route::AdjustSpeedLimit" - CollisionMonitor: + collision_monitor: plugin: "nav2_route::CollisionMonitor" max_collision_dist: 3.0 - edge_cost_functions: ["DistanceScorer", "CostmapScorer"] - DistanceScorer: + edge_cost_functions: ["distance_scorer", "costmap_scorer"] + distance_scorer: plugin: "nav2_route::DistanceScorer" - CostmapScorer: + costmap_scorer: plugin: "nav2_route::CostmapScorer" velocity_smoother: @@ -511,8 +511,8 @@ collision_monitor: stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: + polygons: ["footprint_approach"] + footprint_approach: type: "polygon" action_type: "approach" footprint_topic: "local_costmap/published_footprint" diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index ab0ff781fbe..621d4df2c34 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -5,8 +5,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index 54048f1c171..6054ca7b549 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -7,8 +7,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml index 6560dc4c441..629b1401019 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml @@ -14,8 +14,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 3293b52df45..145ffc6a8ee 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -9,9 +9,9 @@ - - - + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 5a00c52a20f..051d1865ea4 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -10,9 +10,9 @@ - - - + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index 7ddbf6297dd..7d072bd61a5 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -9,8 +9,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index 139da7d9a93..b63562c4fab 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -8,8 +8,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index c500a3ca2e9..a23ae979644 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -5,8 +5,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index d3735fa9447..7c9a176a38f 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -5,8 +5,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index ad56f441325..d2e6f22b795 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -4,8 +4,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index db512b294bd..a7f9f2b635e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -5,8 +5,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 645500fe482..98e782af5d1 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -5,8 +5,8 @@ - - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml index e8cb406c8df..612b29c2ede 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml @@ -17,8 +17,8 @@ - - + + diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 34878429792..635212dffd2 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -6,8 +6,8 @@ collision_detector: transform_tolerance: 0.5 source_timeout: 5.0 base_shift_correction: true - polygons: ["PolygonFront"] - PolygonFront: + polygons: ["polygon_front"] + polygon_front: type: "polygon" points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "none" diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 8b2c6dc26c6..d3207263745 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -16,8 +16,8 @@ collision_monitor: # (2) "circle" type with static footprint set by radius. "footprint_topic" parameter # to be ignored in circular case. # (3) "velocity_polygon" type with dynamically set polygon from velocity_polygons - polygons: ["PolygonStop"] - PolygonStop: + polygons: ["polygon_stop"] + polygon_stop: type: "polygon" points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "stop" @@ -25,7 +25,7 @@ collision_monitor: visualize: true polygon_pub_topic: "polygon_stop" enabled: true - PolygonSlow: + polygon_slow: type: "polygon" points: "[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]" action_type: "slowdown" @@ -34,7 +34,7 @@ collision_monitor: visualize: true polygon_pub_topic: "polygon_slowdown" enabled: true - PolygonLimit: + polygon_limit: type: "polygon" points: "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]" action_type: "limit" @@ -44,7 +44,7 @@ collision_monitor: visualize: true polygon_pub_topic: "polygon_limit" enabled: true - FootprintApproach: + footprint_approach: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" @@ -53,7 +53,7 @@ collision_monitor: min_points: 6 visualize: false enabled: true - VelocityPolygonStop: + velocity_polygon_stop: type: "velocity_polygon" action_type: "stop" min_points: 6 diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 3c1dac9ad29..5e59a1a2c4e 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -432,7 +432,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: source->getSourceTimeout().seconds() != 0.0) { action_polygon = nullptr; - robot_action.polygon_name = "invalid source"; + robot_action.polygon_name = "invalid_source"; robot_action.action_type = STOP; robot_action.req_vel.x = 0.0; robot_action.req_vel.y = 0.0; @@ -606,7 +606,7 @@ void CollisionMonitor::notifyActionState( const Action & robot_action, const std::shared_ptr action_polygon) const { if (robot_action.action_type == STOP) { - if (robot_action.polygon_name == "invalid source") { + if (robot_action.polygon_name == "invalid_source") { RCLCPP_WARN( get_logger(), "Robot to stop due to invalid source." diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 024485b62c9..af85aff6fe1 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -48,11 +48,11 @@ static constexpr double EPSILON = 1e-5; static const char BASE_FRAME_ID[]{"base_link"}; static const char SOURCE_FRAME_ID[]{"base_source"}; static const char ODOM_FRAME_ID[]{"odom"}; -static const char SCAN_NAME[]{"Scan"}; -static const char POINTCLOUD_NAME[]{"PointCloud"}; -static const char RANGE_NAME[]{"Range"}; -static const char POLYGON_NAME[]{"Polygon"}; -static const char COSTMAP_NAME[]{"Costmap"}; +static const char SCAN_NAME[]{"scan"}; +static const char POINTCLOUD_NAME[]{"pointcloud"}; +static const char RANGE_NAME[]{"range"}; +static const char POLYGON_NAME[]{"polygon"}; +static const char COSTMAP_NAME[]{"costmap"}; static const char STATE_TOPIC[]{"collision_detector_state"}; static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_detector/collision_points_marker"}; static const int MIN_POINTS{1}; @@ -559,9 +559,9 @@ bool Tester::waitData( TEST_F(Tester, testIncorrectPolygonType) { setCommonParameters(); - addPolygon("UnknownShape", POLYGON_UNKNOWN, 1.0, "none"); + addPolygon("unknown_shape", POLYGON_UNKNOWN, 1.0, "none"); addSource(SCAN_NAME, SCAN); - setVectors({"UnknownShape"}, {SCAN_NAME}); + setVectors({"unknown_shape"}, {SCAN_NAME}); // Check that Collision Detector node can not be configured for this parameters set cd_->cant_configure(); @@ -570,9 +570,9 @@ TEST_F(Tester, testIncorrectPolygonType) TEST_F(Tester, testIncorrectActionType) { setCommonParameters(); - addPolygon("DetectionRegion", POLYGON, 1.0, "approach"); + addPolygon("detection_region", POLYGON, 1.0, "approach"); addSource(SCAN_NAME, SCAN); - setVectors({"DetectionRegion"}, {SCAN_NAME}); + setVectors({"detection_region"}, {SCAN_NAME}); // Check that Collision Detector node can not be configured for this action type cd_->cant_configure(); @@ -581,9 +581,9 @@ TEST_F(Tester, testIncorrectActionType) TEST_F(Tester, testIncorrectSourceType) { setCommonParameters(); - addPolygon("DetectionRegion", POLYGON, 1.0, "none"); - addSource("UnknownSource", SOURCE_UNKNOWN); - setVectors({"DetectionRegion"}, {"UnknownSource"}); + addPolygon("detection_region", POLYGON, 1.0, "none"); + addSource("unknown_source", SOURCE_UNKNOWN); + setVectors({"detection_region"}, {"unknown_source"}); // Check that Collision Detector node can not be configured for this parameters set cd_->cant_configure(); @@ -592,7 +592,7 @@ TEST_F(Tester, testIncorrectSourceType) TEST_F(Tester, testPolygonsNotSet) { setCommonParameters(); - addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addPolygon("detection_region", POLYGON, 1.0, "none"); addSource(SCAN_NAME, SCAN); // Check that Collision Detector node can not be configured for this parameters set @@ -602,11 +602,11 @@ TEST_F(Tester, testPolygonsNotSet) TEST_F(Tester, testSourcesNotSet) { setCommonParameters(); - addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addPolygon("detection_region", POLYGON, 1.0, "none"); addSource(SCAN_NAME, SCAN); cd_->declare_parameter( "polygons", - rclcpp::ParameterValue(std::vector{"DetectionRegion"})); + rclcpp::ParameterValue(std::vector{"detection_region"})); // Check that Collision Detector node can not be configured for this parameters set cd_->cant_configure(); @@ -615,9 +615,9 @@ TEST_F(Tester, testSourcesNotSet) TEST_F(Tester, testSuccessfulConfigure) { setCommonParameters(); - addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addPolygon("detection_region", POLYGON, 1.0, "none"); addSource(SCAN_NAME, SCAN); - setVectors({"DetectionRegion"}, {SCAN_NAME}); + setVectors({"detection_region"}, {SCAN_NAME}); // Check that Collision Detector node can be configured successfully for this parameters set cd_->configure(); @@ -628,9 +628,9 @@ TEST_F(Tester, testProcessNonActive) rclcpp::Time curr_time = cd_->now(); setCommonParameters(); - addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addPolygon("detection_region", POLYGON, 1.0, "none"); addSource(SCAN_NAME, SCAN); - setVectors({"DetectionRegion"}, {SCAN_NAME}); + setVectors({"detection_region"}, {SCAN_NAME}); // Configure Collision Detector node, but not activate cd_->configure(); @@ -647,9 +647,9 @@ TEST_F(Tester, testProcessActive) rclcpp::Time curr_time = cd_->now(); setCommonParameters(); - addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addPolygon("detection_region", POLYGON, 1.0, "none"); addSource(SCAN_NAME, SCAN); - setVectors({"DetectionRegion"}, {SCAN_NAME}); + setVectors({"detection_region"}, {SCAN_NAME}); // Configure and activate Collision Detector node cd_->start(); @@ -667,9 +667,9 @@ TEST_F(Tester, testPolygonDetection) // Set Collision Detector parameters. setCommonParameters(); // Create polygon - addPolygon("DetectionRegion", POLYGON, 2.0, "none"); + addPolygon("detection_region", POLYGON, 2.0, "none"); addSource(RANGE_NAME, RANGE); - setVectors({"DetectionRegion"}, {RANGE_NAME}); + setVectors({"detection_region"}, {RANGE_NAME}); // Start Collision Detector node cd_->start(); @@ -677,7 +677,7 @@ TEST_F(Tester, testPolygonDetection) // Share TF sendTransforms(curr_time); - // Obstacle is in DetectionRegion + // Obstacle is in detection_region publishRange(1.5, curr_time); ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); @@ -696,9 +696,9 @@ TEST_F(Tester, testCircleDetection) // Set Collision Detector parameters. setCommonParameters(); // Create Circle - addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addPolygon("detection_region", CIRCLE, 3.0, "none"); addSource(RANGE_NAME, RANGE); - setVectors({"DetectionRegion"}, {RANGE_NAME}); + setVectors({"detection_region"}, {RANGE_NAME}); // Start Collision Detector node cd_->start(); @@ -706,7 +706,7 @@ TEST_F(Tester, testCircleDetection) // Share TF sendTransforms(curr_time); - // Obstacle is in DetectionRegion + // Obstacle is in detection_region publishRange(1.5, curr_time); ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); @@ -725,9 +725,9 @@ TEST_F(Tester, testScanDetection) // Set Collision Detector parameters. setCommonParameters(); // Create polygon - addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addPolygon("detection_region", CIRCLE, 3.0, "none"); addSource(SCAN_NAME, SCAN); - setVectors({"DetectionRegion"}, {SCAN_NAME}); + setVectors({"detection_region"}, {SCAN_NAME}); // Start Collision Detector node cd_->start(); @@ -735,7 +735,7 @@ TEST_F(Tester, testScanDetection) // Share TF sendTransforms(curr_time); - // Obstacle is in DetectionRegion + // Obstacle is in detection_region publishScan(1.5, curr_time); ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); @@ -754,9 +754,9 @@ TEST_F(Tester, testPointcloudDetection) // Set Collision Detector parameters. setCommonParameters(); // Create polygon - addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addPolygon("detection_region", CIRCLE, 3.0, "none"); addSource(POINTCLOUD_NAME, POINTCLOUD); - setVectors({"DetectionRegion"}, {POINTCLOUD_NAME}); + setVectors({"detection_region"}, {POINTCLOUD_NAME}); // Start Collision Detector node cd_->start(); @@ -764,7 +764,7 @@ TEST_F(Tester, testPointcloudDetection) // Share TF sendTransforms(curr_time); - // Obstacle is in DetectionRegion + // Obstacle is in detection_region publishPointCloud(2.5, curr_time); ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); @@ -783,9 +783,9 @@ TEST_F(Tester, testPolygonSourceDetection) // Set Collision Detector parameters. setCommonParameters(); // Create polygon - addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addPolygon("detection_region", CIRCLE, 3.0, "none"); addSource(POLYGON_NAME, POLYGON_SOURCE); - setVectors({"DetectionRegion"}, {POLYGON_NAME}); + setVectors({"detection_region"}, {POLYGON_NAME}); // Start Collision Detector node cd_->start(); @@ -793,7 +793,7 @@ TEST_F(Tester, testPolygonSourceDetection) // Share TF sendTransforms(curr_time); - // Obstacle is in DetectionRegion + // Obstacle is in detection_region publishPolygon(2.5, curr_time); ASSERT_TRUE(waitData(std::hypot(2.5, 1.0), 500ms, curr_time)); @@ -812,9 +812,9 @@ TEST_F(Tester, testCostmapDetection) // Set Collision Detector parameters. setCommonParameters(); // Create polygon - addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addPolygon("detection_region", CIRCLE, 3.0, "none"); addSource(COSTMAP_NAME, COSTMAP); - setVectors({"DetectionRegion"}, {COSTMAP_NAME}); + setVectors({"detection_region"}, {COSTMAP_NAME}); // Start Collision Detector node cd_->start(); @@ -822,7 +822,7 @@ TEST_F(Tester, testCostmapDetection) // Share TF sendTransforms(curr_time); - // Obstacle is in DetectionRegion + // Obstacle is in detection_region publishCostmap(1.5, curr_time); ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.yaml b/nav2_collision_monitor/test/collision_monitor_node_bag.yaml index d01ad0d9454..2f7146529ec 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_bag.yaml +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.yaml @@ -17,8 +17,8 @@ collision_monitor: cost_threshold: 253 enabled: true - polygons: [Detection] - Detection: + polygons: [detection] + detection: type: circle action_type: stop min_points: 1 diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index b4042e310e7..1d9997335b5 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -51,10 +51,10 @@ static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; static const char STATE_TOPIC[]{"collision_monitor_state"}; static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_monitor/collision_points_marker"}; static const char FOOTPRINT_TOPIC[]{"footprint"}; -static const char SCAN_NAME[]{"Scan"}; -static const char POINTCLOUD_NAME[]{"PointCloud"}; -static const char RANGE_NAME[]{"Range"}; -static const char POLYGON_NAME[]{"Polygon"}; +static const char SCAN_NAME[]{"scan"}; +static const char POINTCLOUD_NAME[]{"pointcloud"}; +static const char RANGE_NAME[]{"range"}; +static const char POLYGON_NAME[]{"polygon"}; static const int MIN_POINTS{2}; static const double SLOWDOWN_RATIO{0.7}; static const double LINEAR_LIMIT{0.4}; @@ -805,9 +805,9 @@ TEST_F(Tester, testToggleService) { // Set parameters for collision monitor setCommonParameters(); - addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("stop", POLYGON, 1.0, "stop"); addSource(SCAN_NAME, SCAN); - setVectors({"Stop"}, {SCAN_NAME}); + setVectors({"stop"}, {SCAN_NAME}); // Test the parameter in disabled state cm_->set_parameter(rclcpp::Parameter("enabled", false)); @@ -845,11 +845,11 @@ TEST_F(Tester, testProcessStopSlowdownLimit) // Set Collision Monitor parameters. // Making two polygons: outer polygon for slowdown and inner for robot stop. setCommonParameters(); - addPolygon("Limit", POLYGON, 3.0, "limit"); - addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); - addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("limit", POLYGON, 3.0, "limit"); + addPolygon("slowdown", POLYGON, 2.0, "slowdown"); + addPolygon("stop", POLYGON, 1.0, "stop"); addSource(SCAN_NAME, SCAN); - setVectors({"Limit", "SlowDown", "Stop"}, {SCAN_NAME}); + setVectors({"limit", "slowdown", "stop"}, {SCAN_NAME}); // Start Collision Monitor node cm_->start(); @@ -880,7 +880,7 @@ TEST_F(Tester, testProcessStopSlowdownLimit) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, LIMIT); - ASSERT_EQ(action_state_->polygon_name, "Limit"); + ASSERT_EQ(action_state_->polygon_name, "limit"); // 3. Obstacle is in slowdown robot zone publishScan(1.5, curr_time); @@ -892,7 +892,7 @@ TEST_F(Tester, testProcessStopSlowdownLimit) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * SLOWDOWN_RATIO, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, SLOWDOWN); - ASSERT_EQ(action_state_->polygon_name, "SlowDown"); + ASSERT_EQ(action_state_->polygon_name, "slowdown"); // 4. Obstacle is inside stop zone publishScan(0.5, curr_time); @@ -904,7 +904,7 @@ TEST_F(Tester, testProcessStopSlowdownLimit) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "Stop"); + ASSERT_EQ(action_state_->polygon_name, "stop"); // 5. Restoring back normal operation publishScan(4.5, curr_time); @@ -932,11 +932,11 @@ TEST_F(Tester, testPolygonSource) // Set source_timeout to 0.0 to clear out quickly the polygons from test to test cm_->set_parameter( rclcpp::Parameter("source_timeout", 0.1)); - addPolygon("Limit", POLYGON, 3.0, "limit"); - addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); - addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("limit", POLYGON, 3.0, "limit"); + addPolygon("slowdown", POLYGON, 2.0, "slowdown"); + addPolygon("stop", POLYGON, 1.0, "stop"); addSource(POLYGON_NAME, POLYGON_SOURCE); - setVectors({"Limit", "SlowDown", "Stop"}, {POLYGON_NAME}); + setVectors({"limit", "slowdown", "stop"}, {POLYGON_NAME}); // Start Collision Monitor node cm_->start(); @@ -969,7 +969,7 @@ TEST_F(Tester, testPolygonSource) EXPECT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON); EXPECT_TRUE(waitActionState(500ms)); EXPECT_EQ(action_state_->action_type, LIMIT); - EXPECT_EQ(action_state_->polygon_name, "Limit"); + EXPECT_EQ(action_state_->polygon_name, "limit"); // 3. Obstacle is in slowdown robot zone curr_time = cm_->now(); @@ -982,7 +982,7 @@ TEST_F(Tester, testPolygonSource) EXPECT_NEAR(cmd_vel_out_->angular.z, 0.1 * SLOWDOWN_RATIO, EPSILON); EXPECT_TRUE(waitActionState(500ms)); EXPECT_EQ(action_state_->action_type, SLOWDOWN); - EXPECT_EQ(action_state_->polygon_name, "SlowDown"); + EXPECT_EQ(action_state_->polygon_name, "slowdown"); // 4. Obstacle is inside stop zone curr_time = cm_->now(); @@ -995,7 +995,7 @@ TEST_F(Tester, testPolygonSource) EXPECT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); EXPECT_TRUE(waitActionState(500ms)); EXPECT_EQ(action_state_->action_type, STOP); - EXPECT_EQ(action_state_->polygon_name, "Stop"); + EXPECT_EQ(action_state_->polygon_name, "stop"); // 5. Restoring back normal operation curr_time = cm_->now(); @@ -1021,9 +1021,9 @@ TEST_F(Tester, testProcessApproach) // Set Collision Monitor parameters. // Making one circle footprint for approach. setCommonParameters(); - addPolygon("Approach", CIRCLE, 1.0, "approach"); + addPolygon("approach", CIRCLE, 1.0, "approach"); addSource(SCAN_NAME, SCAN); - setVectors({"Approach"}, {SCAN_NAME}); + setVectors({"approach"}, {SCAN_NAME}); // Start Collision Monitor node cm_->start(); @@ -1055,7 +1055,7 @@ TEST_F(Tester, testProcessApproach) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, APPROACH); - ASSERT_EQ(action_state_->polygon_name, "Approach"); + ASSERT_EQ(action_state_->polygon_name, "approach"); // 3. Obstacle is inside robot footprint publishScan(0.5, curr_time); @@ -1090,9 +1090,9 @@ TEST_F(Tester, testProcessApproachRotation) // Set Collision Monitor parameters. // Making one circle footprint for approach. setCommonParameters(); - addPolygon("Approach", POLYGON, 1.0, "approach"); + addPolygon("approach", POLYGON, 1.0, "approach"); addSource(RANGE_NAME, RANGE); - setVectors({"Approach"}, {RANGE_NAME}); + setVectors({"approach"}, {RANGE_NAME}); // Start Collision Monitor node cm_->start(); @@ -1127,7 +1127,7 @@ TEST_F(Tester, testProcessApproachRotation) (M_PI / 4) * (SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION)); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, APPROACH); - ASSERT_EQ(action_state_->polygon_name, "Approach"); + ASSERT_EQ(action_state_->polygon_name, "approach"); // 3. Obstacle is inside robot footprint publishRange(0.5, curr_time); @@ -1162,11 +1162,11 @@ TEST_F(Tester, testCrossOver) // Making two polygons: outer polygon for slowdown and inner circle // as robot footprint for approach. setCommonParameters(); - addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); - addPolygon("Approach", CIRCLE, 1.0, "approach"); + addPolygon("slowdown", POLYGON, 2.0, "slowdown"); + addPolygon("approach", CIRCLE, 1.0, "approach"); addSource(POINTCLOUD_NAME, POINTCLOUD); addSource(RANGE_NAME, RANGE); - setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); + setVectors({"slowdown", "approach"}, {POINTCLOUD_NAME, RANGE_NAME}); // Start Collision Monitor node cm_->start(); @@ -1189,7 +1189,7 @@ TEST_F(Tester, testCrossOver) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, APPROACH); - ASSERT_EQ(action_state_->polygon_name, "Approach"); + ASSERT_EQ(action_state_->polygon_name, "approach"); // 2. Obstacle is inside slowdown zone, but speed is too slow for approach publishRange(1.5, curr_time); @@ -1201,7 +1201,7 @@ TEST_F(Tester, testCrossOver) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, SLOWDOWN); - ASSERT_EQ(action_state_->polygon_name, "SlowDown"); + ASSERT_EQ(action_state_->polygon_name, "slowdown"); // 3. Increase robot speed to return again into approach mode. // The speed should be safer for approach mode, so robot will go to the approach (ahead in 0.5 m) @@ -1216,7 +1216,7 @@ TEST_F(Tester, testCrossOver) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, APPROACH); - ASSERT_EQ(action_state_->polygon_name, "Approach"); + ASSERT_EQ(action_state_->polygon_name, "approach"); // Stop Collision Monitor node cm_->stop(); @@ -1230,11 +1230,11 @@ TEST_F(Tester, testSourceTimeout) // Making two polygons: outer polygon for slowdown and inner circle // as robot footprint for approach. setCommonParameters(); - addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); - addPolygon("Approach", CIRCLE, 1.0, "approach"); + addPolygon("slowdown", POLYGON, 2.0, "slowdown"); + addPolygon("approach", CIRCLE, 1.0, "approach"); addSource(POINTCLOUD_NAME, POINTCLOUD); addSource(RANGE_NAME, RANGE); - setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); + setVectors({"slowdown", "approach"}, {POINTCLOUD_NAME, RANGE_NAME}); // Start Collision Monitor node cm_->start(); @@ -1254,7 +1254,7 @@ TEST_F(Tester, testSourceTimeout) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "invalid source"); + ASSERT_EQ(action_state_->polygon_name, "invalid_source"); // Stop Collision Monitor node cm_->stop(); @@ -1268,11 +1268,11 @@ TEST_F(Tester, testSourceTimeoutOverride) // Making two polygons: outer polygon for slowdown and inner circle // as robot footprint for approach. setCommonParameters(); - addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); - addPolygon("Approach", CIRCLE, 1.0, "approach"); + addPolygon("slowdown", POLYGON, 2.0, "slowdown"); + addPolygon("approach", CIRCLE, 1.0, "approach"); addSource(POINTCLOUD_NAME, POINTCLOUD); addSource(RANGE_NAME, RANGE); - setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); + setVectors({"slowdown", "approach"}, {POINTCLOUD_NAME, RANGE_NAME}); cm_->set_parameter(rclcpp::Parameter("source_timeout", 0.0)); // Start Collision Monitor node @@ -1297,7 +1297,7 @@ TEST_F(Tester, testSourceTimeoutOverride) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, APPROACH); - ASSERT_EQ(action_state_->polygon_name, "Approach"); + ASSERT_EQ(action_state_->polygon_name, "approach"); // Stop Collision Monitor node cm_->stop(); @@ -1309,10 +1309,10 @@ TEST_F(Tester, testCeasePublishZeroVel) // Configure stop and approach zones, and basic data source setCommonParameters(); - addPolygon("Stop", POLYGON, 1.0, "stop"); - addPolygon("Approach", CIRCLE, 2.0, "approach"); + addPolygon("stop", POLYGON, 1.0, "stop"); + addPolygon("approach", CIRCLE, 2.0, "approach"); addSource(SCAN_NAME, SCAN); - setVectors({"Stop", "Approach"}, {SCAN_NAME}); + setVectors({"stop", "approach"}, {SCAN_NAME}); // Start Collision Monitor node cm_->start(); @@ -1330,7 +1330,7 @@ TEST_F(Tester, testCeasePublishZeroVel) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, APPROACH); - ASSERT_EQ(action_state_->polygon_name, "Approach"); + ASSERT_EQ(action_state_->polygon_name, "approach"); // Wait more than STOP_PUB_TIMEOUT time std::this_thread::sleep_for(std::chrono::duration(STOP_PUB_TIMEOUT + 0.01)); @@ -1361,7 +1361,7 @@ TEST_F(Tester, testCeasePublishZeroVel) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "Stop"); + ASSERT_EQ(action_state_->polygon_name, "stop"); // Wait more than STOP_PUB_TIMEOUT time std::this_thread::sleep_for(std::chrono::duration(STOP_PUB_TIMEOUT + 0.01)); @@ -1379,10 +1379,10 @@ TEST_F(Tester, testPolygonNotEnabled) // Set Collision Monitor parameters. // Create a STOP polygon setCommonParameters(); - addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("stop", POLYGON, 1.0, "stop"); // Create a Scan source addSource(SCAN_NAME, SCAN); - setVectors({"Stop"}, {SCAN_NAME}); + setVectors({"stop"}, {SCAN_NAME}); // Start Collision Monitor node cm_->start(); @@ -1399,12 +1399,12 @@ TEST_F(Tester, testPolygonNotEnabled) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "Stop"); + ASSERT_EQ(action_state_->polygon_name, "stop"); // Disable polygon by calling service auto set_parameters_msg = std::make_shared(); auto parameter_msg = std::make_shared(); - parameter_msg->name = "Stop.enabled"; + parameter_msg->name = "stop.enabled"; parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; parameter_msg->value.bool_value = false; set_parameters_msg->parameters.push_back(*parameter_msg); @@ -1434,10 +1434,10 @@ TEST_F(Tester, testSourceNotEnabled) // Set Collision Monitor parameters. // Create a STOP polygon setCommonParameters(); - addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("stop", POLYGON, 1.0, "stop"); // Create a Scan source addSource(SCAN_NAME, SCAN); - setVectors({"Stop"}, {SCAN_NAME}); + setVectors({"stop"}, {SCAN_NAME}); // Start Collision Monitor node cm_->start(); @@ -1454,7 +1454,7 @@ TEST_F(Tester, testSourceNotEnabled) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "Stop"); + ASSERT_EQ(action_state_->polygon_name, "stop"); // Disable source by calling service auto set_parameters_msg = std::make_shared(); @@ -1489,9 +1489,9 @@ TEST_F(Tester, testProcessNonActive) rclcpp::Time curr_time = cm_->now(); setCommonParameters(); - addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("stop", POLYGON, 1.0, "stop"); addSource(SCAN_NAME, SCAN); - setVectors({"Stop"}, {SCAN_NAME}); + setVectors({"stop"}, {SCAN_NAME}); // Configure Collision Monitor node, but not activate cm_->configure(); @@ -1511,9 +1511,9 @@ TEST_F(Tester, testProcessNonActive) TEST_F(Tester, testIncorrectPolygonType) { setCommonParameters(); - addPolygon("UnknownShape", POLYGON_UNKNOWN, 1.0, "stop"); + addPolygon("unknown_shape", POLYGON_UNKNOWN, 1.0, "stop"); addSource(SCAN_NAME, SCAN); - setVectors({"UnknownShape"}, {SCAN_NAME}); + setVectors({"unknown_shape"}, {SCAN_NAME}); // Check that Collision Monitor node can not be configured for this parameters set cm_->cant_configure(); @@ -1522,9 +1522,9 @@ TEST_F(Tester, testIncorrectPolygonType) TEST_F(Tester, testIncorrectSourceType) { setCommonParameters(); - addPolygon("Stop", POLYGON, 1.0, "stop"); - addSource("UnknownSource", SOURCE_UNKNOWN); - setVectors({"Stop"}, {"UnknownSource"}); + addPolygon("stop", POLYGON, 1.0, "stop"); + addSource("unknown_source", SOURCE_UNKNOWN); + setVectors({"stop"}, {"unknown_source"}); // Check that Collision Monitor node can not be configured for this parameters set cm_->cant_configure(); @@ -1533,7 +1533,7 @@ TEST_F(Tester, testIncorrectSourceType) TEST_F(Tester, testPolygonsNotSet) { setCommonParameters(); - addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("stop", POLYGON, 1.0, "stop"); addSource(SCAN_NAME, SCAN); // Check that Collision Monitor node can not be configured for this parameters set @@ -1543,9 +1543,9 @@ TEST_F(Tester, testPolygonsNotSet) TEST_F(Tester, testSourcesNotSet) { setCommonParameters(); - addPolygon("Stop", POLYGON, 1.0, "stop"); + addPolygon("stop", POLYGON, 1.0, "stop"); addSource(SCAN_NAME, SCAN); - cm_->declare_parameter("polygons", rclcpp::ParameterValue(std::vector{"Stop"})); + cm_->declare_parameter("polygons", rclcpp::ParameterValue(std::vector{"stop"})); // Check that Collision Monitor node can not be configured for this parameters set cm_->cant_configure(); @@ -1585,15 +1585,15 @@ TEST_F(Tester, testVelocityPolygonStop) { // Set Collision Monitor parameters. // Add velocity polygon with 2 sub polygon: - // 1. Forward: 0 -> 0.5 m/s - // 2. Backward: 0 -> -0.5 m/s + // 1. forward: 0 -> 0.5 m/s + // 2. backward: 0 -> -0.5 m/s setCommonParameters(); - addPolygon("VelocityPolygon", VELOCITY_POLYGON, 1.0, "stop"); - addPolygonVelocitySubPolygon("VelocityPolygon", "Forward", 0.0, 0.5, 0.0, 1.0, 4.0); - addPolygonVelocitySubPolygon("VelocityPolygon", "Backward", -0.5, 0.0, 0.0, 1.0, 2.0); - setPolygonVelocityVectors("VelocityPolygon", {"Forward", "Backward"}); + addPolygon("velocity_polygon", VELOCITY_POLYGON, 1.0, "stop"); + addPolygonVelocitySubPolygon("velocity_polygon", "forward", 0.0, 0.5, 0.0, 1.0, 4.0); + addPolygonVelocitySubPolygon("velocity_polygon", "backward", -0.5, 0.0, 0.0, 1.0, 2.0); + setPolygonVelocityVectors("velocity_polygon", {"forward", "backward"}); addSource(POINTCLOUD_NAME, POINTCLOUD); - setVectors({"VelocityPolygon"}, {POINTCLOUD_NAME}); + setVectors({"velocity_polygon"}, {POINTCLOUD_NAME}); rclcpp::Time curr_time = cm_->now(); // Start Collision Monitor node @@ -1601,7 +1601,7 @@ TEST_F(Tester, testVelocityPolygonStop) // Check that robot stops when source is enabled sendTransforms(curr_time); - // 1. Obstacle is far away from Forward velocity polygon + // 1. Obstacle is far away from forward velocity polygon publishPointCloud(4.5, curr_time); ASSERT_TRUE(waitData(std::hypot(4.5, 0.01), 500ms, curr_time)); publishCmdVel(0.4, 0.0, 0.1); @@ -1610,7 +1610,7 @@ TEST_F(Tester, testVelocityPolygonStop) ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); - // 2. Obstacle is in Forward velocity polygon + // 2. Obstacle is in forward velocity polygon publishPointCloud(3.0, curr_time); ASSERT_TRUE(waitData(std::hypot(3.0, 0.01), 500ms, curr_time)); publishCmdVel(0.4, 0.0, 0.1); @@ -1620,10 +1620,10 @@ TEST_F(Tester, testVelocityPolygonStop) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "VelocityPolygon"); + ASSERT_EQ(action_state_->polygon_name, "velocity_polygon"); - // 3. Switch to Backward velocity polygon - // Obstacle is far away from Backward velocity polygon + // 3. Switch to backward velocity polygon + // Obstacle is far away from backward velocity polygon publishCmdVel(-0.4, 0.0, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, -0.4, EPSILON); @@ -1633,7 +1633,7 @@ TEST_F(Tester, testVelocityPolygonStop) ASSERT_EQ(action_state_->action_type, DO_NOTHING); ASSERT_EQ(action_state_->polygon_name, ""); - // 4. Obstacle is in Backward velocity polygon + // 4. Obstacle is in backward velocity polygon publishPointCloud(-1.5, curr_time); ASSERT_TRUE(waitData(std::hypot(-1.5, 0.01), 500ms, curr_time)); publishCmdVel(-0.4, 0.0, 0.1); @@ -1643,7 +1643,7 @@ TEST_F(Tester, testVelocityPolygonStop) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "VelocityPolygon"); + ASSERT_EQ(action_state_->polygon_name, "velocity_polygon"); // Stop Collision Monitor node cm_->stop(); @@ -1653,16 +1653,16 @@ TEST_F(Tester, testVelocityPolygonStopGlobalHeight) { // Set Collision Monitor parameters. // Add velocity polygon with 2 sub polygon: - // 1. Forward: 0 -> 0.5 m/s - // 2. Backward: 0 -> -0.5 m/s + // 1. forward: 0 -> 0.5 m/s + // 2. backward: 0 -> -0.5 m/s setCommonParameters(); - addPolygon("VelocityPolygon", VELOCITY_POLYGON, 1.0, "stop"); - addPolygonVelocitySubPolygon("VelocityPolygon", "Forward", 0.0, 0.5, 0.0, 1.0, 4.0); - addPolygonVelocitySubPolygon("VelocityPolygon", "Backward", -0.5, 0.0, 0.0, 1.0, 2.0); - setPolygonVelocityVectors("VelocityPolygon", {"Forward", "Backward"}); + addPolygon("velocity_polygon", VELOCITY_POLYGON, 1.0, "stop"); + addPolygonVelocitySubPolygon("velocity_polygon", "forward", 0.0, 0.5, 0.0, 1.0, 4.0); + addPolygonVelocitySubPolygon("velocity_polygon", "backward", -0.5, 0.0, 0.0, 1.0, 2.0); + setPolygonVelocityVectors("velocity_polygon", {"forward", "backward"}); addSource(POINTCLOUD_NAME, POINTCLOUD); setGlobalHeightParams(POINTCLOUD_NAME, 0.5); - setVectors({"VelocityPolygon"}, {POINTCLOUD_NAME}); + setVectors({"velocity_polygon"}, {POINTCLOUD_NAME}); cm_->set_parameter( rclcpp::Parameter("source_timeout", 2.0)); @@ -1673,7 +1673,7 @@ TEST_F(Tester, testVelocityPolygonStopGlobalHeight) // Check that robot stops when source is enabled sendTransforms(curr_time); - // 1. Obstacle is in Forward velocity polygon and below global height + // 1. Obstacle is in forward velocity polygon and below global height publishPointCloudWithHeight(3.0, 0.4, curr_time); ASSERT_FALSE(waitData(std::hypot(3.0, 0.01), 500ms, curr_time)); publishCmdVel(0.4, 0.0, 0.1); @@ -1682,7 +1682,7 @@ TEST_F(Tester, testVelocityPolygonStopGlobalHeight) ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); - // 2. Obstacle is in Forward velocity polygon and above global height + // 2. Obstacle is in forward velocity polygon and above global height publishPointCloudWithHeight(3.0, 0.6, curr_time); ASSERT_TRUE(waitData(std::hypot(3.0, 0.01), 500ms, curr_time)); publishCmdVel(0.4, 0.0, 0.1); @@ -1692,7 +1692,7 @@ TEST_F(Tester, testVelocityPolygonStopGlobalHeight) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "VelocityPolygon"); + ASSERT_EQ(action_state_->polygon_name, "velocity_polygon"); // 3. Pointcloud without height field, invalid source. publishPointCloud(2.5, curr_time); @@ -1701,7 +1701,7 @@ TEST_F(Tester, testVelocityPolygonStopGlobalHeight) ASSERT_FALSE(waitCmdVel(500ms)); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "invalid source"); + ASSERT_EQ(action_state_->polygon_name, "invalid_source"); // Stop Collision Monitor node cm_->stop(); @@ -1718,9 +1718,9 @@ TEST_F(Tester, testSourceAssociatedToPolygon) addSource(RANGE_NAME, RANGE); std::vector range_only_sources_names = {RANGE_NAME}; std::vector all_sources_names = {SCAN_NAME, RANGE_NAME}; - addPolygon("StopOnRangeSource", POLYGON, 1.0, "stop", range_only_sources_names); - addPolygon("SlowdownOnAllSources", POLYGON, 1.0, "slowdown"); - setVectors({"StopOnRangeSource", "SlowdownOnAllSources"}, {SCAN_NAME, RANGE_NAME}); + addPolygon("stop_on_range_source", POLYGON, 1.0, "stop", range_only_sources_names); + addPolygon("slowdown_on_all_sources", POLYGON, 1.0, "slowdown"); + setVectors({"stop_on_range_source", "slowdown_on_all_sources"}, {SCAN_NAME, RANGE_NAME}); // Start Collision Monitor node cm_->start(); @@ -1743,7 +1743,7 @@ TEST_F(Tester, testSourceAssociatedToPolygon) // Since the stop polygon is only checking range source, slowdown action should be applied ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, SLOWDOWN); - ASSERT_EQ(action_state_->polygon_name, "SlowdownOnAllSources"); + ASSERT_EQ(action_state_->polygon_name, "slowdown_on_all_sources"); // Stop Collision Monitor node cm_->stop(); diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index c161f60f528..b05e04e2bad 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -44,8 +44,8 @@ static const char BASE2_FRAME_ID[]{"base2_link"}; static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char POLYGON_SUB_TOPIC[]{"polygon_sub"}; static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; -static const char POLYGON_NAME[]{"TestPolygon"}; -static const char CIRCLE_NAME[]{"TestCircle"}; +static const char POLYGON_NAME[]{"test_polygon"}; +static const char CIRCLE_NAME[]{"test_circle"}; static const char OBSERVATION_SOURCE_NAME[]{"source"}; static const std::vector SQUARE_POLYGON { 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}; diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 89b0a792f92..e2084c35f90 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -50,15 +50,15 @@ static constexpr double EPSILON = std::numeric_limits::epsilon(); static const char BASE_FRAME_ID[]{"base_link"}; static const char SOURCE_FRAME_ID[]{"base_source"}; static const char GLOBAL_FRAME_ID[]{"odom"}; -static const char SCAN_NAME[]{"LaserScan"}; +static const char SCAN_NAME[]{"laser_scan"}; static const char SCAN_TOPIC[]{"scan"}; -static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char POINTCLOUD_NAME[]{"pointcloud"}; static const char POINTCLOUD_TOPIC[]{"pointcloud"}; -static const char RANGE_NAME[]{"Range"}; +static const char RANGE_NAME[]{"range"}; static const char RANGE_TOPIC[]{"range"}; -static const char POLYGON_NAME[]{"Polygon"}; +static const char POLYGON_NAME[]{"polygon"}; static const char POLYGON_TOPIC[]{"polygon"}; -static const char COSTMAP_NAME[]{"Costmap"}; +static const char COSTMAP_NAME[]{"costmap"}; static const char COSTMAP_TOPIC[]{"costmap"}; static const char INVALID_FRAME_ID[]{"invalid_frame"}; static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 201c6c65e95..909aee9c6be 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -41,11 +41,11 @@ static constexpr double EPSILON = std::numeric_limits::epsilon(); static const char BASE_FRAME_ID[]{"base_link"}; static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; -static const char POLYGON_NAME[]{"TestVelocityPolygon"}; -static const char SUB_POLYGON_FORWARD_NAME[]{"Forward"}; -static const char SUB_POLYGON_BACKWARD_NAME[]{"Backward"}; -static const char SUB_POLYGON_LEFT_NAME[]{"Left"}; -static const char SUB_POLYGON_RIGHT_NAME[]{"Right"}; +static const char POLYGON_NAME[]{"test_velocity_polygon"}; +static const char SUB_POLYGON_FORWARD_NAME[]{"forward"}; +static const char SUB_POLYGON_BACKWARD_NAME[]{"backward"}; +static const char SUB_POLYGON_LEFT_NAME[]{"left"}; +static const char SUB_POLYGON_RIGHT_NAME[]{"right"}; static const std::vector FORWARD_POLYGON{ 0.5, 0.5, 0.5, -0.5, 0.0, -0.5, 0.0, 0.5}; static const std::vector BACKWARD_POLYGON{ diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md index 72768ede25e..9f8ad7d1274 100644 --- a/nav2_constrained_smoother/README.md +++ b/nav2_constrained_smoother/README.md @@ -10,9 +10,9 @@ Example of configuration (see indoor_navigation package of this repo for a full ``` smoother_server: ros__parameters: - smoother_plugins: ["SmoothPath"] + smoother_plugins: ["smooth_path"] - SmoothPath: + smooth_path: plugin: "nav2_constrained_smoother/ConstrainedSmoother" reversing_enabled: true # whether to detect forward/reverse direction and cusps. Should be set to false for paths without orientations assigned path_downsampling_factor: 3 # every n-th node of the path is taken. Useful for speed-up diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 6e4e3d4bc90..1d1f7edf702 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -143,27 +143,27 @@ class SmootherTest : public ::testing::Test smoother_ = std::make_shared(); smoother_->configure( - node_lifecycle_, "SmoothPath", tf_buffer_, costmap_sub_, + node_lifecycle_, "smooth_path", tf_buffer_, costmap_sub_, std::shared_ptr()); smoother_->activate(); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 2000000.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.minimum_turning_radius", 0.4)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 30.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_dist", 0.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", -1.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost_cusp_multiplier", 1.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_downsampling_factor", 1)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 1)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.reversing_enabled", true)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.keep_start_orientation", true)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.keep_goal_orientation", true)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_smooth", 2000000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.minimum_turning_radius", 0.4)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_curve", 30.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_dist", 0.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_cost", 0.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.cusp_zone_length", -1.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_cost_cusp_multiplier", 1.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.path_downsampling_factor", 1)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.path_upsampling_factor", 1)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.reversing_enabled", true)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.keep_start_orientation", true)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.keep_goal_orientation", true)); node_lifecycle_->set_parameter( - rclcpp::Parameter("SmoothPath.optimizer.linear_solver_type", "SPARSE_NORMAL_CHOLESKY")); + rclcpp::Parameter("smooth_path.optimizer.linear_solver_type", "SPARSE_NORMAL_CHOLESKY")); node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.cost_check_points", + "smooth_path.cost_check_points", std::vector())); reloadParams(); } @@ -184,7 +184,7 @@ class SmootherTest : public ::testing::Test smoother_->deactivate(); smoother_->cleanup(); smoother_->configure( - node_lifecycle_, "SmoothPath", tf_buffer_, costmap_sub_, + node_lifecycle_, "smooth_path", tf_buffer_, costmap_sub_, std::shared_ptr()); smoother_->activate(); } @@ -418,7 +418,7 @@ class SmootherTest : public ::testing::Test TEST_F(SmootherTest, testingSmoothness) { // set w_curve to 0.0 so that the whole job is upon w_smooth - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 0.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_curve", 0.0)); reloadParams(); std::vector sharp_turn_90 = @@ -485,9 +485,9 @@ TEST_F(SmootherTest, testingSmoothness) TEST_F(SmootherTest, testingAnchoringToOriginalPath) { - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 30.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_smooth", 30.0)); // set w_curve to 0.0, we don't care about turning radius in this test... - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 0.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_curve", 0.0)); // first keep w_dist at 0.0 to generate an unanchored smoothed path reloadParams(); @@ -505,7 +505,7 @@ TEST_F(SmootherTest, testingAnchoringToOriginalPath) EXPECT_TRUE(smoothPath(sharp_turn_90, smoothed_path)); // then update w_dist and compare the results - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_dist", 30.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_dist", 30.0)); reloadParams(); std::vector smoothed_path_anchored; @@ -525,11 +525,11 @@ TEST_F(SmootherTest, testingAnchoringToOriginalPath) TEST_F(SmootherTest, testingMaxCurvature) { - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 30.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_curve", 30.0)); // set w_smooth to a small value so that the whole job is upon w_curve - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 0.3)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_smooth", 0.3)); // let's give the smoother more time since w_smooth is so small - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.optimizer.max_iterations", 500)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.optimizer.max_iterations", 500)); reloadParams(); // smoother should increase radius from infeasible 0.3 to feasible 0.4 @@ -620,8 +620,8 @@ TEST_F(SmootherTest, testingObstacleAvoidance) footprint.push_back(pointMsg(-0.4, -0.25)); footprint.push_back(pointMsg(0.4, -0.25)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 2000000.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_smooth", 2000000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_cost", 0.015)); reloadParams(); std::vector straight_near_obstacle = @@ -729,15 +729,15 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) footprint.push_back(pointMsg(0.4, -0.2)); // first smooth with homogeneous w_cost to compare - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_smooth", 15000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_cost", 0.015)); // higher w_curve significantly decreases convergence speed here // path feasibility can be restored by subsequent resmoothing with higher w_curve // TODO(afrixs): tune ceres optimizer to "converge" faster, // see http://ceres-solver.org/nnls_solving.html - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 1.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_curve", 1.0)); // let's have more iterations so that the improvement is more significant - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.optimizer.max_iterations", 500)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.optimizer.max_iterations", 500)); reloadParams(); std::vector smoothed_path; @@ -761,11 +761,11 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) // then update parameters so that robot is not so afraid of obstacles // during simple movement but pays extra attention during rotations near cusps - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.0052)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_cost", 0.0052)); node_lifecycle_->set_parameter( - rclcpp::Parameter("SmoothPath.w_cost_cusp_multiplier", 0.027 / 0.0052)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", 2.5)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.optimizer.fn_tol", 1e-15)); + rclcpp::Parameter("smooth_path.w_cost_cusp_multiplier", 0.027 / 0.0052)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.cusp_zone_length", 2.5)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.optimizer.fn_tol", 1e-15)); reloadParams(); std::vector smoothed_path_ecc; @@ -816,13 +816,13 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) footprint.push_back(pointMsg(0.15, -0.2)); // reset parameters back to homogeneous and shift cost check point to the center of the footprint - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 1.0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", -1.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_smooth", 15000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_curve", 1.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_cost", 0.015)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.cusp_zone_length", -1.0)); node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.cost_check_points", + "smooth_path.cost_check_points", std::vector({-0.05, 0.0, 0.5, -0.45, 0.0, 0.5}) // x1, y1, weight1, x2, y2, weight2 )); reloadParams(); @@ -868,7 +868,7 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) // (testing automatic weights normalization, i.e. using avg instead of sum) node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.cost_check_points", + "smooth_path.cost_check_points", std::vector({-0.05, 0.0, 1.0, -0.45, 0.0, 1.0}) // x1, y1, weight1, x2, y2, weight2 )); reloadParams(); @@ -879,14 +879,14 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) //////////////////////////////////////// // compare also with extra careful cusp - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.0052)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.w_cost", 0.0052)); node_lifecycle_->set_parameter( - rclcpp::Parameter("SmoothPath.w_cost_cusp_multiplier", 0.027 / 0.0052)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", 2.5)); + rclcpp::Parameter("smooth_path.w_cost_cusp_multiplier", 0.027 / 0.0052)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.cusp_zone_length", 2.5)); // we need much more iterations here since it's a more complicated problem // TODO(afrixs): tune ceres optimizer to "converge" faster // see http://ceres-solver.org/nnls_solving.html - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.optimizer.max_iterations", 1500)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.optimizer.max_iterations", 1500)); reloadParams(); std::vector smoothed_path_scce; @@ -912,7 +912,7 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) // resmooth extra careful cusp with same conditions (higher max_iterations) node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.cost_check_points", + "smooth_path.cost_check_points", std::vector())); reloadParams(); @@ -960,24 +960,24 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) }; cusp_i_ = 6; - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_downsampling_factor", 2)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.path_downsampling_factor", 2)); // downsample only - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 0)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.reversing_enabled", false)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.path_upsampling_factor", 0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.reversing_enabled", false)); reloadParams(); std::vector smoothed_path_downsampled; EXPECT_TRUE(smoothPath(sharp_turn_90_then_reverse, smoothed_path_downsampled)); // first two, last two and every 2nd pose between them EXPECT_EQ(smoothed_path_downsampled.size(), 8u); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.reversing_enabled", true)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.reversing_enabled", true)); reloadParams(); EXPECT_TRUE(smoothPath(sharp_turn_90_then_reverse, smoothed_path_downsampled)); // same but downsampling is reset on cusp EXPECT_EQ(smoothed_path_downsampled.size(), 9u); // upsample to original size - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 1)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.path_upsampling_factor", 1)); reloadParams(); std::vector smoothed_path; EXPECT_TRUE(smoothPath(sharp_turn_90_then_reverse, smoothed_path)); @@ -1005,7 +1005,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) EXPECT_NEAR(smoothness_improvement, 63.9, 1.0); // upsample above original size - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 2)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.path_upsampling_factor", 2)); reloadParams(); EXPECT_TRUE(smoothPath(sharp_turn_90_then_reverse, smoothed_path)); // every pose except last produces 2 poses @@ -1045,8 +1045,8 @@ TEST_F(SmootherTest, testingStartGoalOrientations) EXPECT_NEAR(smoothed_path.back()[2], M_PI / 2, 0.001); // Overwrite start/goal orientations - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.keep_start_orientation", false)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.keep_goal_orientation", false)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.keep_start_orientation", false)); + node_lifecycle_->set_parameter(rclcpp::Parameter("smooth_path.keep_goal_orientation", false)); reloadParams(); sharp_turn_90[0][2] = M_PI; // forward/reverse of start pose should not matter @@ -1086,19 +1086,19 @@ TEST_F(SmootherTest, testingCostCheckPointsParamValidity) { node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.cost_check_points", + "smooth_path.cost_check_points", std::vector())); reloadParams(); node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.cost_check_points", + "smooth_path.cost_check_points", std::vector({0, 0, 0, 0, 0, 0, 0, 0, 0}))); // multiple of 3 is ok reloadParams(); node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.cost_check_points", + "smooth_path.cost_check_points", std::vector({0, 0}))); EXPECT_THROW(reloadParams(), std::runtime_error); } @@ -1107,19 +1107,19 @@ TEST_F(SmootherTest, testingLinearSolverTypeParamValidity) { node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.optimizer.linear_solver_type", + "smooth_path.optimizer.linear_solver_type", "SPARSE_NORMAL_CHOLESKY")); reloadParams(); node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.optimizer.linear_solver_type", + "smooth_path.optimizer.linear_solver_type", "DENSE_QR")); reloadParams(); node_lifecycle_->set_parameter( rclcpp::Parameter( - "SmoothPath.optimizer.linear_solver_type", + "smooth_path.optimizer.linear_solver_type", "INVALID_SOLVER")); EXPECT_THROW(reloadParams(), std::runtime_error); } diff --git a/nav2_controller/include/nav2_controller/parameter_handler.hpp b/nav2_controller/include/nav2_controller/parameter_handler.hpp index b0df84bb851..33cc10ffc99 100644 --- a/nav2_controller/include/nav2_controller/parameter_handler.hpp +++ b/nav2_controller/include/nav2_controller/parameter_handler.hpp @@ -93,9 +93,9 @@ class ParameterHandler : public nav2_util::ParameterHandler "nav2_controller::SimpleProgressChecker"}; const std::vector default_goal_checker_ids_{"goal_checker"}; const std::vector default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}; - const std::vector default_controller_ids_{"FollowPath"}; + const std::vector default_controller_ids_{"follow_path"}; const std::vector default_controller_types_{"nav2_mppi_controller::MPPIController"}; - const std::vector default_path_handler_ids_{"PathHandler"}; + const std::vector default_path_handler_ids_{"path_handler"}; const std::vector default_path_handler_types_{ "nav2_controller::FeasiblePathHandler"}; }; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2545457df2b..35881e1d41b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -85,14 +85,14 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) nav2_core::ProgressChecker::Ptr progress_checker = progress_checker_loader_.createUniqueInstance(params_->progress_checker_types[i]); RCLCPP_INFO( - get_logger(), "Created progress_checker : %s of type %s", + get_logger(), "Created progress checker : %s of type %s", params_->progress_checker_ids[i].c_str(), params_->progress_checker_types[i].c_str()); progress_checker->initialize(node, params_->progress_checker_ids[i]); progress_checkers_.insert({params_->progress_checker_ids[i], progress_checker}); } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), - "Failed to create progress_checker. Exception: %s", ex.what()); + "Failed to create progress checker. Exception: %s", ex.what()); on_cleanup(state); return nav2::CallbackReturn::FAILURE; } @@ -328,7 +328,7 @@ bool ControllerServer::findControllerId( current_controller = controllers_.begin()->first; } else { RCLCPP_ERROR( - get_logger(), "FollowPath called with controller name %s, " + get_logger(), "follow_path called with controller name %s, " "which does not exist. Available controllers are: %s.", c_name.c_str(), controller_ids_concat_.c_str()); return false; @@ -354,7 +354,7 @@ bool ControllerServer::findGoalCheckerId( current_goal_checker = goal_checkers_.begin()->first; } else { RCLCPP_ERROR( - get_logger(), "FollowPath called with goal_checker name %s in parameter" + get_logger(), "follow_path called with goal checker name %s in parameter" " 'current_goal_checker', which does not exist. Available goal checkers are: %s.", c_name.c_str(), goal_checker_ids_concat_.c_str()); return false; @@ -380,7 +380,7 @@ bool ControllerServer::findProgressCheckerId( return true; } else { RCLCPP_ERROR( - get_logger(), "FollowPath called with progress_checker name %s in parameter" + get_logger(), "follow_path called with progress checker name %s in parameter" " 'current_progress_checker', but no progress checkers are configured.", c_name.c_str()); return false; @@ -396,7 +396,7 @@ bool ControllerServer::findProgressCheckerId( current_progress_checker = progress_checkers_.begin()->first; } else { RCLCPP_ERROR( - get_logger(), "FollowPath called with progress_checker name %s in parameter" + get_logger(), "follow_path called with progress checker name %s in parameter" " 'current_progress_checker', which does not exist. Available progress checkers are: %s.", c_name.c_str(), progress_checker_ids_concat_.c_str()); return false; @@ -422,7 +422,7 @@ bool ControllerServer::findPathHandlerId( current_path_handler = path_handlers_.begin()->first; } else { RCLCPP_ERROR( - get_logger(), "FollowPath called with path_handler name %s in parameter" + get_logger(), "follow_path called with path_handler name %s in parameter" " 'current_path_handler', which does not exist. Available path handlers are: %s.", c_name.c_str(), path_handler_ids_concat_.c_str()); return false; diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 6726dc9582a..daa26857a63 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -335,7 +335,7 @@ void ObstacleLayer::onInitialize() if (inf_is_valid) { RCLCPP_WARN( logger_, - "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); + "ObstacleLayer: inf_is_valid option is not applicable to PointCloud observations."); } auto filter = std::make_shared>( diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml index de412e00121..114890b211a 100644 --- a/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml +++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml @@ -1,5 +1,5 @@ docks: dock1: type: "dockv3" - frame: "mapA" + frame: "map_a" pose: [0.3, 0.3] diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml index 2e92bf51add..28396345126 100644 --- a/nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml +++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml @@ -1,7 +1,7 @@ docks: dock1: type: "dockv3" - frame: "mapA" + frame: "map_a" pose: [0.3, 0.3, 0.0] dock2: type: "dockv1" diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml index 4d88a4da695..a3efefc6c4e 100644 --- a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml +++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml @@ -1,4 +1,4 @@ docks: dock1: type: "dockv3" - frame: "mapA" + frame: "map_a" diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml index 197e691c7af..8c6057e8d6a 100644 --- a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml +++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml @@ -1,4 +1,4 @@ docks: dock1: - frame: "mapA" + frame: "map_a" pose: [0.3, 0.3, 0.0] diff --git a/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml index 49195f2cb4c..20a9c9977fb 100644 --- a/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml +++ b/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml @@ -1,5 +1,5 @@ no_docks: dock1: type: "dockv3" - frame: "mapA" + frame: "map_a" pose: [0.3, 0.3, 0.0] diff --git a/nav2_docking/opennav_docking/test/test_utils.cpp b/nav2_docking/opennav_docking/test/test_utils.cpp index e3607643a9b..a1b0328ba4e 100644 --- a/nav2_docking/opennav_docking/test/test_utils.cpp +++ b/nav2_docking/opennav_docking/test/test_utils.cpp @@ -26,7 +26,7 @@ TEST(UtilsTests, parseDockParams1) { auto node = std::make_shared("test"); DockMap db; - std::vector dock_str = {"dockA", "dockB"}; + std::vector dock_str = {"dock_a", "dock_b"}; node->declare_parameter("docks", rclcpp::ParameterValue(dock_str)); std::vector docks_param; @@ -40,50 +40,50 @@ TEST(UtilsTests, parseDockParams2) { auto node = std::make_shared("test2"); DockMap db; - std::vector dock_str = {"dockC", "dockD"}; + std::vector dock_str = {"dock_c", "dock_d"}; node->declare_parameter("docks", rclcpp::ParameterValue(dock_str)); - // Don't declare B, check if "map" default - node->declare_parameter("dockC.frame", rclcpp::ParameterValue(std::string("mapA"))); + // Don't declare dock_d.frame, check if "map" default + node->declare_parameter("dock_c.frame", rclcpp::ParameterValue(std::string("map_a"))); - node->declare_parameter("dockC.type", rclcpp::ParameterValue(std::string("typeA"))); - node->declare_parameter("dockD.type", rclcpp::ParameterValue(std::string("typeB"))); + node->declare_parameter("dock_c.type", rclcpp::ParameterValue(std::string("type_a"))); + node->declare_parameter("dock_d.type", rclcpp::ParameterValue(std::string("type_b"))); std::vector dock_pose = {0.3, 0.3, 0.3}; - node->declare_parameter("dockC.pose", rclcpp::ParameterValue(dock_pose)); - node->declare_parameter("dockD.pose", rclcpp::ParameterValue(dock_pose)); + node->declare_parameter("dock_c.pose", rclcpp::ParameterValue(dock_pose)); + node->declare_parameter("dock_d.pose", rclcpp::ParameterValue(dock_pose)); - // Don't declare C, check if empty string default - node->declare_parameter("dockD.id", rclcpp::ParameterValue("D")); + // Don't declare dock_c.id, check if empty string default + node->declare_parameter("dock_d.id", rclcpp::ParameterValue("d")); std::vector docks_param; node->get_parameter("docks", docks_param); EXPECT_TRUE(utils::parseDockParams(docks_param, node, db)); - EXPECT_EQ(db["dockC"].frame, std::string("mapA")); - EXPECT_EQ(db["dockD"].frame, std::string("map")); - EXPECT_EQ(db["dockC"].type, std::string("typeA")); - EXPECT_EQ(db["dockC"].pose.position.x, 0.3); - EXPECT_EQ(db["dockC"].pose.position.y, 0.3); - EXPECT_EQ(db["dockC"].id, std::string("")); - EXPECT_EQ(db["dockD"].id, std::string("D")); + EXPECT_EQ(db["dock_c"].frame, std::string("map_a")); + EXPECT_EQ(db["dock_d"].frame, std::string("map")); + EXPECT_EQ(db["dock_c"].type, std::string("type_a")); + EXPECT_EQ(db["dock_c"].pose.position.x, 0.3); + EXPECT_EQ(db["dock_c"].pose.position.y, 0.3); + EXPECT_EQ(db["dock_c"].id, std::string("")); + EXPECT_EQ(db["dock_d"].id, std::string("d")); } TEST(UtilsTests, parseDockParams3) { auto node = std::make_shared("test3"); DockMap db; - std::vector dock_str = {"dockE", "dockF"}; + std::vector dock_str = {"dock_e", "dock_f"}; node->declare_parameter("docks", rclcpp::ParameterValue(dock_str)); - node->declare_parameter("dockE.type", rclcpp::ParameterValue(std::string("typeA"))); - node->declare_parameter("dockF.type", rclcpp::ParameterValue(std::string("typeB"))); + node->declare_parameter("dock_e.type", rclcpp::ParameterValue(std::string("type_a"))); + node->declare_parameter("dock_f.type", rclcpp::ParameterValue(std::string("type_b"))); std::vector docks_param; node->get_parameter("docks", docks_param); // Incorrect pose size std::vector dock_pose_err = {0.3, 0.3}; - node->declare_parameter("dockE.pose", rclcpp::ParameterValue(dock_pose_err)); + node->declare_parameter("dock_e.pose", rclcpp::ParameterValue(dock_pose_err)); EXPECT_FALSE(utils::parseDockParams(docks_param, node, db)); } @@ -95,7 +95,7 @@ TEST(UtilsTests, parseDockFile) "/dock_files/test_dock_file.yaml"; EXPECT_TRUE(utils::parseDockFile(filepath, node, db)); EXPECT_EQ(db.size(), 2u); - EXPECT_EQ(db["dock1"].frame, std::string("mapA")); + EXPECT_EQ(db["dock1"].frame, std::string("map_a")); EXPECT_EQ(db["dock2"].frame, std::string("map")); EXPECT_EQ(db["dock1"].type, std::string("dockv3")); EXPECT_EQ(db["dock2"].type, std::string("dockv1")); diff --git a/nav2_map_server/params/vector_object_server_params.yaml b/nav2_map_server/params/vector_object_server_params.yaml index e369a6240fe..8ed8253fddd 100644 --- a/nav2_map_server/params/vector_object_server_params.yaml +++ b/nav2_map_server/params/vector_object_server_params.yaml @@ -7,14 +7,14 @@ vector_object_server: overlay_type: 0 update_frequency: 1.0 transform_tolerance: 0.1 - shapes: ["Poly", "Circle"] - Poly: + 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: + circle: type: "circle" frame_id: "map" fill: true diff --git a/nav2_map_server/test/unit/test_vector_object_server.cpp b/nav2_map_server/test/unit/test_vector_object_server.cpp index 8e3616abe25..9b84b8f21a3 100644 --- a/nav2_map_server/test/unit/test_vector_object_server.cpp +++ b/nav2_map_server/test/unit/test_vector_object_server.cpp @@ -42,8 +42,8 @@ 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 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}; diff --git a/nav2_map_server/test/unit/test_vector_object_shapes.cpp b/nav2_map_server/test/unit/test_vector_object_shapes.cpp index 43a6f43a58d..ff8c2dd331f 100644 --- a/nav2_map_server/test/unit/test_vector_object_shapes.cpp +++ b/nav2_map_server/test/unit/test_vector_object_shapes.cpp @@ -39,8 +39,8 @@ static constexpr double EPSILON = std::numeric_limits::epsilon(); -static const char POLYGON_NAME[]{"Polygon"}; -static const char CIRCLE_NAME[]{"Circle"}; +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; diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 282ac229e25..f5a9241a677 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -183,7 +183,7 @@ Uses inflated costmap cost directly to avoid obstacles controller_server: ros__parameters: controller_frequency: 30.0 - FollowPath: + follow_path: plugin: "nav2_mppi_controller::MPPIController" time_steps: 56 model_dt: 0.05 @@ -314,7 +314,7 @@ There also exists a relationship between the costmap configurations and the Obst Thus, care should be taken to select weights of the obstacle critic in conjunction with the costmap inflation radius and scale so that a robot does not have such issues. How I (Steve, your friendly neighborhood navigator) tuned this was to first create the appropriate obstacle critic behavior desirable in conjunction with the inflation layer parameters. Its worth noting that the Obstacle critic converts the cost into a distance from obstacles, so the nature of the distribution of costs in the inflation isn't overly significant. However, the inflation radius and the scale will define the cost at the end of the distribution where free-space meets the lowest cost value within the radius. So testing for quality behavior when going over that threshold should be considered. -As you increase or decrease your weights on the Obstacle, you may notice the aforementioned behaviors (e.g. won't overcome free to non-free threshold). To overcome them, increase the FollowPath critic cost to increase the desire for the trajectory planner to continue moving towards the goal. Make sure to not overshoot this though, keep them balanced. A desirable outcome is smooth motion roughly in the center of spaces without significant close interactions with obstacles. It shouldn't be perfectly following a path yet nor should the output velocity be wobbling jaggedly. +As you increase or decrease your weights on the Obstacle, you may notice the aforementioned behaviors (e.g. won't overcome free to non-free threshold). To overcome them, increase the follow_path critic cost to increase the desire for the trajectory planner to continue moving towards the goal. Make sure to not overshoot this though, keep them balanced. A desirable outcome is smooth motion roughly in the center of spaces without significant close interactions with obstacles. It shouldn't be perfectly following a path yet nor should the output velocity be wobbling jaggedly. 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. diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ff274c83bb8..ffbf96323b0 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -44,7 +44,7 @@ namespace nav2_planner PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) : nav2::LifecycleNode("planner_server", "", options), gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), - default_ids_{"GridBased"}, + default_ids_{"grid_based"}, default_types_{"nav2_navfn_planner::NavfnPlanner"}, partial_plan_allowed_{false}, costmap_update_timeout_(1s), @@ -721,7 +721,7 @@ void PlannerServer::exceptionWarning( { std::stringstream ss; ss << std::fixed << std::setprecision(2) - << planner_id << "plugin failed to plan from (" + << planner_id << " plugin failed to plan from (" << start.pose.position.x << ", " << start.pose.position.y << ") to (" << goal.pose.position.x << ", " << goal.pose.position.y << ")" diff --git a/nav2_planner/test/test_plan_through_poses.cpp b/nav2_planner/test/test_plan_through_poses.cpp index f0aa854c07c..0fc384cdac7 100644 --- a/nav2_planner/test/test_plan_through_poses.cpp +++ b/nav2_planner/test/test_plan_through_poses.cpp @@ -27,7 +27,7 @@ // Some parameters for planner server static const double EXPECTED_PLANNER_FREQ{20.0}; -static const std::vector PLANNER_PLUGINS{"GridBased"}; +static const std::vector PLANNER_PLUGINS{"grid_based"}; static const double COSTMAP_UPDATE_TIMEOUT{1.0}; static const char PLANNER_PLUGIN_NAME[]{"nav2_smac_planner::SmacPlanner2D"}; static const double PLANNER_TOLERANCE{0.1}; diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 4a3f632dba5..cb6e29e74b7 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -101,8 +101,8 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: "goal_checker" - controller_plugins: ["FollowPath"] + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["follow_path"] progress_checker: plugin: "nav2_controller::SimpleProgressChecker" @@ -113,7 +113,7 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True - FollowPath: + follow_path: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" desired_linear_vel: 0.5 lookahead_dist: 0.6 diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index aec04163dc0..bb72b6a4097 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -82,7 +82,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure TEST(RegulatedPurePursuitTest, basicAPI) { auto node = std::make_shared("testRPP"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); @@ -127,7 +127,7 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared(); auto node = std::make_shared("testRPP"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); rclcpp_lifecycle::State state; @@ -169,9 +169,9 @@ TEST(RegulatedPurePursuitTest, rotateTests) auto ctrl = std::make_shared(); auto node = std::make_shared("testRPP"); nav2::declare_parameter_if_not_declared( - node, "PathFollower.stateful", rclcpp::ParameterValue(false)); + node, "path_follower.stateful", rclcpp::ParameterValue(false)); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); rclcpp_lifecycle::State state; @@ -241,7 +241,7 @@ TEST(RegulatedPurePursuitTest, rotateTests) // Stateful Configuration // ----------------------- node->set_parameter( - rclcpp::Parameter("PathFollower.stateful", true)); + rclcpp::Parameter("path_follower.stateful", true)); ctrl->configure(node, name, tf, costmap); @@ -263,7 +263,7 @@ TEST(RegulatedPurePursuitTest, applyConstraints) { auto ctrl = std::make_shared(); auto node = std::make_shared("testRPP"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); rclcpp_lifecycle::State state; diff --git a/nav2_ros_common/test/test_node_utils.cpp b/nav2_ros_common/test/test_node_utils.cpp index a8e565e44b8..7d71c3badb0 100644 --- a/nav2_ros_common/test/test_node_utils.cpp +++ b/nav2_ros_common/test/test_node_utils.cpp @@ -125,8 +125,8 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) { ::testing::FLAGS_gtest_death_test_style = "threadsafe"; auto node = std::make_shared("test_node"); - node->declare_parameter("Foo.plugin", "bar"); - ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); + node->declare_parameter("foo.plugin", "Bar"); + ASSERT_EQ(get_plugin_type_param(node, "foo"), "Bar"); EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error); } diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 3a19898a40a..0705c2cb96c 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -50,8 +50,8 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: "goal_checker" - controller_plugins: ["FollowPath"] + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["follow_path"] progress_checker: plugin: "nav2_controller::SimpleProgressChecker" @@ -62,7 +62,7 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True - FollowPath: + follow_path: plugin: "nav2_rotation_shim_controller::RotationShimController" primary_controller: "dwb_core::DWBLocalPlanner" angular_dist_threshold: 0.785 diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index b2fbedd498a..5d1d7b9b87f 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -70,7 +70,7 @@ TEST(RotationShimControllerTest, lifecycleTransitions) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); rclcpp_lifecycle::State state; @@ -87,7 +87,7 @@ TEST(RotationShimControllerTest, lifecycleTransitions) node->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter( - "PathFollower.primary_controller.plugin", + "path_follower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"))}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -108,7 +108,7 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); rclcpp_lifecycle::State state; @@ -116,10 +116,10 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller.plugin", + "path_follower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); // disable collision check - node->declare_parameter("PathFollower.simulate_ahead_time", 0.0); + node->declare_parameter("path_follower.simulate_ahead_time", 0.0); auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); @@ -155,14 +155,14 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap", "/", false); costmap->configure(); // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller.plugin", + "path_follower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter("controller_frequency", 1.0); @@ -215,7 +215,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto listener = std::make_shared(*tf, node, true); auto costmap = std::make_shared("fake_costmap"); @@ -235,7 +235,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller.plugin", + "path_follower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter("controller_frequency", 1.0); @@ -280,7 +280,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) TEST(RotationShimControllerTest, openLoopRotationTests) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto listener = std::make_shared(*tf, node, true); auto costmap = std::make_shared("fake_costmap"); @@ -299,16 +299,16 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller.plugin", + "path_follower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter( "controller_frequency", 20.0); node->declare_parameter( - "PathFollower.rotate_to_goal_heading", + "path_follower.rotate_to_goal_heading", true); node->declare_parameter( - "PathFollower.closed_loop", + "path_follower.closed_loop", false); auto controller = std::make_shared(); @@ -367,7 +367,7 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto listener = std::make_shared(*tf, node, true); auto costmap = std::make_shared("fake_costmap"); @@ -386,13 +386,13 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller.plugin", + "path_follower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter( - "PathFollower.rotate_to_goal_heading", + "path_follower.rotate_to_goal_heading", true); node->declare_parameter("controller_frequency", 1.0); - node->declare_parameter("PathFollower.primary_controller.use_collision_detection", false); + node->declare_parameter("path_follower.primary_controller.use_collision_detection", false); auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); @@ -450,7 +450,7 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { TEST(RotationShimControllerTest, accelerationTests) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto listener = std::make_shared(*tf, node, true); auto costmap = std::make_shared("fake_costmap"); @@ -469,16 +469,16 @@ TEST(RotationShimControllerTest, accelerationTests) { // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller.plugin", + "path_follower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter( "controller_frequency", 20.0); node->declare_parameter( - "PathFollower.rotate_to_goal_heading", + "path_follower.rotate_to_goal_heading", true); node->declare_parameter( - "PathFollower.max_angular_accel", + "path_follower.max_angular_accel", 0.5); auto controller = std::make_shared(); @@ -538,7 +538,7 @@ TEST(RotationShimControllerTest, isGoalChangedTest) { auto ctrl = std::make_shared(); auto node = std::make_shared("ShimControllerTest"); - std::string name = "PathFollower"; + std::string name = "path_follower"; auto tf = std::make_shared(node->get_clock()); auto listener = std::make_shared(*tf, node, true); auto costmap = std::make_shared("fake_costmap"); @@ -560,13 +560,13 @@ TEST(RotationShimControllerTest, isGoalChangedTest) // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller.plugin", + "path_follower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter( - "PathFollower.rotate_to_heading_once", + "path_follower.rotate_to_heading_once", true); // disable collision check - node->declare_parameter("PathFollower.simulate_ahead_time", 0.0); + node->declare_parameter("path_follower.simulate_ahead_time", 0.0); auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); diff --git a/nav2_route/README.md b/nav2_route/README.md index b1c878637bd..60293fd6f3a 100644 --- a/nav2_route/README.md +++ b/nav2_route/README.md @@ -118,20 +118,20 @@ route_server: smoothing_radius: 1.0 # Radius of corner to fit into the corner costmap_topic: 'global_costmap/costmap_raw' # Costmap topic when enable_nn_search is enabled. May also be used by the collision monitor operation and/or the costmap edge scorer if using the same topic to share resources. - graph_file_loader: "GeoJsonGraphFileLoader" # Name of default file loader + graph_file_loader: "geo_json_graph_file_loader" # Name of default file loader plugin: nav2_route::GeoJsonGraphFileLoader # file loader plugin to use graph_filepath: "" # file path to graph to use - edge_cost_functions: ["DistanceScorer", "DynamicEdgesScorer"] # Edge scoring cost functions to use - DistanceScorer: + edge_cost_functions: ["distance_scorer", "dynamic_edges_scorer"] # Edge scoring cost functions to use + distance_scorer: plugin: "nav2_route::DistanceScorer" - DynamicEdgesScorer: + dynamic_edges_scorer: plugin: "nav2_route::DynamicEdgesScorer" - operations: ["AdjustSpeedLimit", "ReroutingService"] # Route operations plugins to use - AdjustSpeedLimit: + operations: ["adjust_speed_limit", "rerouting_service"] # Route operations plugins to use + adjust_speed_limit: plugin: "nav2_route::AdjustSpeedLimit" - ReroutingService: + rerouting_service: plugin: "nav2_route::ReroutingService" tracker_update_rate: 50.0 # Rate at which to check the status of path tracking @@ -278,8 +278,8 @@ This route operation will trigger an external service when a graph node or edge | Service | Description | |--------------------------------------------------|----------------------------------------------------------------| | `route_server/set_route_graph` | Sets new route graph to use | -| `route_server//adjust_edges` | Sets adjust edge values and closure from 3rd party application | -| `route_server//reroute` | Trigger a reroute from a 3rd party | +| `route_server//adjust_edges` | Sets adjust edge values and closure from 3rd party application | +| `route_server//reroute` | Trigger a reroute from a 3rd party | | Action | Description | |----------------------------------|-----------------------------------------------------------------------| diff --git a/nav2_route/src/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp index 1bfd3830ed8..45faca3ee14 100644 --- a/nav2_route/src/edge_scorer.cpp +++ b/nav2_route/src/edge_scorer.cpp @@ -29,7 +29,7 @@ EdgeScorer::EdgeScorer( : plugin_loader_("nav2_route", "nav2_route::EdgeCostFunction") { // load plugins with a default of the DistanceScorer - const std::vector default_plugin_ids({"DistanceScorer", "DynamicEdgesScorer"}); + const std::vector default_plugin_ids({"distance_scorer", "dynamic_edges_scorer"}); const std::vector default_plugin_types( {"nav2_route::DistanceScorer", "nav2_route::DynamicEdgesScorer"}); diff --git a/nav2_route/src/graph_loader.cpp b/nav2_route/src/graph_loader.cpp index 1c71012e866..7e0968f357c 100644 --- a/nav2_route/src/graph_loader.cpp +++ b/nav2_route/src/graph_loader.cpp @@ -25,7 +25,7 @@ GraphLoader::GraphLoader( std::shared_ptr tf, const std::string frame) : plugin_loader_("nav2_route", "nav2_route::GraphFileLoader"), - default_plugin_id_("GeoJsonGraphFileLoader") + default_plugin_id_("geo_json_graph_file_loader") { logger_ = node->get_logger(); tf_ = tf; diff --git a/nav2_route/src/operations_manager.cpp b/nav2_route/src/operations_manager.cpp index eba58a7f029..4503ed1c75d 100644 --- a/nav2_route/src/operations_manager.cpp +++ b/nav2_route/src/operations_manager.cpp @@ -33,7 +33,7 @@ OperationsManager::OperationsManager( // Have some default operations const std::vector default_plugin_ids( - {"AdjustSpeedLimit", "ReroutingService"}); + {"adjust_speed_limit", "rerouting_service"}); const std::vector default_plugin_types( {"nav2_route::AdjustSpeedLimit", "nav2_route::ReroutingService"}); diff --git a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp index 0cf787a2f1b..885853cf1a6 100644 --- a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp @@ -71,7 +71,7 @@ void CostmapScorer::configure( node, costmap_topic); RCLCPP_INFO( node->get_logger(), - "Using costmap topic: %s instead of server costmap topic: %s for CostmapScorer.", + "Using costmap topic: %s instead of server costmap topic: %s for costmap scorer.", costmap_topic.c_str(), server_costmap_topic.c_str()); } else { costmap_subscriber_ = costmap_subscriber; diff --git a/nav2_route/src/plugins/route_operations/collision_monitor.cpp b/nav2_route/src/plugins/route_operations/collision_monitor.cpp index c45db02d092..1248dcbedab 100644 --- a/nav2_route/src/plugins/route_operations/collision_monitor.cpp +++ b/nav2_route/src/plugins/route_operations/collision_monitor.cpp @@ -39,7 +39,7 @@ void CollisionMonitor::configure( if (costmap_topic != server_costmap_topic) { RCLCPP_INFO( node->get_logger(), - "Using costmap topic: %s instead of server costmap topic: %s for CollisionMonitor.", + "Using costmap topic: %s instead of server costmap topic: %s for collision monitor.", costmap_topic.c_str(), server_costmap_topic.c_str()); costmap_subscriber_ = std::make_shared(node, costmap_topic); topic_ = costmap_topic; diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp index 8c16ced8306..407955f9e63 100644 --- a/nav2_route/test/test_edge_scorers.cpp +++ b/nav2_route/test/test_edge_scorers.cpp @@ -91,9 +91,9 @@ TEST(EdgeScorersTest, test_failed_api) std::shared_ptr tf_buffer; node->declare_parameter( - "edge_cost_functions", rclcpp::ParameterValue(std::vector{"FakeScorer"})); + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"fake_scorer"})); nav2::declare_parameter_if_not_declared( - node, "FakeScorer.plugin", rclcpp::ParameterValue(std::string{"FakePluginPath"})); + node, "fake_scorer.plugin", rclcpp::ParameterValue(std::string{"FakePluginPath"})); std::shared_ptr costmap_subscriber; EXPECT_THROW( @@ -110,9 +110,10 @@ TEST(EdgeScorersTest, test_invalid_edge_scoring) std::shared_ptr tf_buffer; node->declare_parameter( - "edge_cost_functions", rclcpp::ParameterValue(std::vector{"DynamicEdgesScorer"})); + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"dynamic_edges_scorer"})); nav2::declare_parameter_if_not_declared( - node, "DynamicEdgesScorer.plugin", + node, "dynamic_edges_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::DynamicEdgesScorer"})); std::shared_ptr costmap_subscriber; @@ -122,7 +123,7 @@ TEST(EdgeScorersTest, test_invalid_edge_scoring) // Send service to set an edge as invalid auto srv_client = nav2::ServiceClient( - "route_server/DynamicEdgesScorer/adjust_edges", node2); + "route_server/dynamic_edges_scorer/adjust_edges", node2); auto req = std::make_shared(); req->closed_edges.push_back(10u); req->adjust_edges.resize(1); @@ -178,9 +179,9 @@ TEST(EdgeScorersTest, test_penalty_scoring) std::shared_ptr tf_buffer; node->declare_parameter( - "edge_cost_functions", rclcpp::ParameterValue(std::vector{"PenaltyScorer"})); + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"penalty_scorer"})); nav2::declare_parameter_if_not_declared( - node, "PenaltyScorer.plugin", + node, "penalty_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::PenaltyScorer"})); std::shared_ptr costmap_subscriber; EdgeScorer scorer(node, tf_buffer, costmap_subscriber); @@ -217,9 +218,9 @@ TEST(EdgeScorersTest, test_costmap_scoring) std::shared_ptr tf_buffer; node->declare_parameter( - "edge_cost_functions", rclcpp::ParameterValue(std::vector{"CostmapScorer"})); + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"costmap_scorer"})); nav2::declare_parameter_if_not_declared( - node, "CostmapScorer.plugin", + node, "costmap_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::CostmapScorer"})); std::shared_ptr costmap_subscriber; @@ -332,16 +333,16 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) std::shared_ptr tf_buffer; node->declare_parameter( - "edge_cost_functions", rclcpp::ParameterValue(std::vector{"CostmapScorer"})); + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"costmap_scorer"})); nav2::declare_parameter_if_not_declared( - node, "CostmapScorer.plugin", + node, "costmap_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::CostmapScorer"})); node->declare_parameter( - "CostmapScorer.use_maximum", rclcpp::ParameterValue(false)); + "costmap_scorer.use_maximum", rclcpp::ParameterValue(false)); node->declare_parameter( - "CostmapScorer.invalid_on_collision", rclcpp::ParameterValue(false)); + "costmap_scorer.invalid_on_collision", rclcpp::ParameterValue(false)); node->declare_parameter( - "CostmapScorer.invalid_off_map", rclcpp::ParameterValue(false)); + "costmap_scorer.invalid_off_map", rclcpp::ParameterValue(false)); std::shared_ptr costmap_subscriber; EdgeScorer scorer(node, tf_buffer, costmap_subscriber); @@ -433,9 +434,9 @@ TEST(EdgeScorersTest, test_time_scoring) std::shared_ptr tf_buffer; node->declare_parameter( - "edge_cost_functions", rclcpp::ParameterValue(std::vector{"TimeScorer"})); + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"time_scorer"})); nav2::declare_parameter_if_not_declared( - node, "TimeScorer.plugin", + node, "time_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::TimeScorer"})); std::shared_ptr costmap_subscriber; @@ -491,9 +492,9 @@ TEST(EdgeScorersTest, test_semantic_scoring_key) std::shared_ptr tf_buffer; node->declare_parameter( - "edge_cost_functions", rclcpp::ParameterValue(std::vector{"SemanticScorer"})); + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"semantic_scorer"})); nav2::declare_parameter_if_not_declared( - node, "SemanticScorer.plugin", + node, "semantic_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::SemanticScorer"})); std::vector classes; @@ -501,12 +502,12 @@ TEST(EdgeScorersTest, test_semantic_scoring_key) classes.push_back("Test1"); classes.push_back("Test2"); nav2::declare_parameter_if_not_declared( - node, "SemanticScorer.semantic_classes", + node, "semantic_scorer.semantic_classes", rclcpp::ParameterValue(classes)); for (unsigned int i = 0; i != classes.size(); i++) { nav2::declare_parameter_if_not_declared( - node, "SemanticScorer." + classes[i], + node, "semantic_scorer." + classes[i], rclcpp::ParameterValue(static_cast(i))); } @@ -564,12 +565,12 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) std::shared_ptr tf_buffer; node->declare_parameter( - "edge_cost_functions", rclcpp::ParameterValue(std::vector{"SemanticScorer"})); + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"semantic_scorer"})); nav2::declare_parameter_if_not_declared( - node, "SemanticScorer.plugin", + node, "semantic_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::SemanticScorer"})); nav2::declare_parameter_if_not_declared( - node, "SemanticScorer.semantic_key", + node, "semantic_scorer.semantic_key", rclcpp::ParameterValue(std::string{""})); std::vector classes; @@ -577,12 +578,12 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) classes.push_back("Test1"); classes.push_back("Test2"); nav2::declare_parameter_if_not_declared( - node, "SemanticScorer.semantic_classes", + node, "semantic_scorer.semantic_classes", rclcpp::ParameterValue(classes)); for (unsigned int i = 0; i != classes.size(); i++) { nav2::declare_parameter_if_not_declared( - node, "SemanticScorer." + classes[i], + node, "semantic_scorer." + classes[i], rclcpp::ParameterValue(static_cast(i))); } @@ -644,15 +645,15 @@ TEST(EdgeScorersTest, test_goal_orientation_threshold) node->declare_parameter( "edge_cost_functions", - rclcpp::ParameterValue(std::vector{"GoalOrientationScorer"})); + rclcpp::ParameterValue(std::vector{"goal_orientation_scorer"})); nav2::declare_parameter_if_not_declared( - node, "GoalOrientationScorer.plugin", + node, "goal_orientation_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::GoalOrientationScorer"})); nav2::declare_parameter_if_not_declared( - node, "GoalOrientationScorer.orientation_tolerance", + node, "goal_orientation_scorer.orientation_tolerance", rclcpp::ParameterValue(1.57)); nav2::declare_parameter_if_not_declared( - node, "GoalOrientationScorer.use_orientation_threshold", + node, "goal_orientation_scorer.use_orientation_threshold", rclcpp::ParameterValue(true)); std::shared_ptr costmap_subscriber; @@ -715,18 +716,18 @@ TEST(EdgeScorersTest, test_goal_orientation_scoring) node->declare_parameter( "edge_cost_functions", - rclcpp::ParameterValue(std::vector{"GoalOrientationScorer"})); + rclcpp::ParameterValue(std::vector{"goal_orientation_scorer"})); nav2::declare_parameter_if_not_declared( - node, "GoalOrientationScorer.plugin", + node, "goal_orientation_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::GoalOrientationScorer"})); nav2::declare_parameter_if_not_declared( - node, "GoalOrientationScorer.orientation_tolerance", + node, "goal_orientation_scorer.orientation_tolerance", rclcpp::ParameterValue(1.57)); nav2::declare_parameter_if_not_declared( - node, "GoalOrientationScorer.use_orientation_thershold", + node, "goal_orientation_scorer.use_orientation_thershold", rclcpp::ParameterValue(false)); nav2::declare_parameter_if_not_declared( - node, "GoalOrientationScorer.orientation_weight", + node, "goal_orientation_scorer.orientation_weight", rclcpp::ParameterValue(orientation_weight)); @@ -790,15 +791,15 @@ TEST(EdgeScorersTest, test_start_pose_orientation_threshold) node->declare_parameter( "edge_cost_functions", - rclcpp::ParameterValue(std::vector{"StartPoseOrientationScorer"})); + rclcpp::ParameterValue(std::vector{"start_pose_orientation_scorer"})); nav2::declare_parameter_if_not_declared( - node, "StartPoseOrientationScorer.plugin", + node, "start_pose_orientation_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::StartPoseOrientationScorer"})); nav2::declare_parameter_if_not_declared( - node, "StartPoseOrientationScorer.orientation_tolerance", + node, "start_pose_orientation_scorer.orientation_tolerance", rclcpp::ParameterValue(1.57)); nav2::declare_parameter_if_not_declared( - node, "StartPoseOrientationScorer.use_orientation_threshold", + node, "start_pose_orientation_scorer.use_orientation_threshold", rclcpp::ParameterValue(true)); std::shared_ptr costmap_subscriber; @@ -878,18 +879,18 @@ TEST(EdgeScorersTest, test_start_pose_orientation_scoring) node->declare_parameter( "edge_cost_functions", - rclcpp::ParameterValue(std::vector{"StartPoseOrientationScorer"})); + rclcpp::ParameterValue(std::vector{"start_pose_orientation_scorer"})); nav2::declare_parameter_if_not_declared( - node, "StartPoseOrientationScorer.plugin", + node, "start_pose_orientation_scorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::StartPoseOrientationScorer"})); nav2::declare_parameter_if_not_declared( - node, "StartPoseOrientationScorer.orientation_tolerance", + node, "start_pose_orientation_scorer.orientation_tolerance", rclcpp::ParameterValue(1.57)); nav2::declare_parameter_if_not_declared( - node, "StartPoseOrientationScorer.use_orientation_thershold", + node, "start_pose_orientation_scorer.use_orientation_thershold", rclcpp::ParameterValue(false)); nav2::declare_parameter_if_not_declared( - node, "StartPoseOrientationScorer.orientation_weight", + node, "start_pose_orientation_scorer.orientation_weight", rclcpp::ParameterValue(orientation_weight)); std::shared_ptr costmap_subscriber; diff --git a/nav2_route/test/test_operations.cpp b/nav2_route/test/test_operations.cpp index 3b35fea6b96..bdd98584b98 100644 --- a/nav2_route/test/test_operations.cpp +++ b/nav2_route/test/test_operations.cpp @@ -153,14 +153,14 @@ TEST(OperationsManagerTest, test_processing_speed_on_status) // No status change, shouldn't do anything OperationsResult result = manager.process(false, state, route, pose, info); EXPECT_FALSE(result.reroute); - EXPECT_EQ(result.operations_triggered.size(), 1u); // ReroutingService - EXPECT_EQ(result.operations_triggered[0], std::string("ReroutingService")); + EXPECT_EQ(result.operations_triggered.size(), 1u); // rerouting_service + EXPECT_EQ(result.operations_triggered[0], std::string("rerouting_service")); // Status change, may now trigger the only plugin result = manager.process(true, state, route, pose, info); EXPECT_EQ(result.operations_triggered.size(), 2u); - EXPECT_EQ(result.operations_triggered[0], std::string("AdjustSpeedLimit")); - EXPECT_EQ(result.operations_triggered[1], std::string("ReroutingService")); + EXPECT_EQ(result.operations_triggered[0], std::string("adjust_speed_limit")); + EXPECT_EQ(result.operations_triggered[1], std::string("rerouting_service")); rclcpp::Rate r(10); r.sleep(); @@ -178,9 +178,9 @@ TEST(OperationsManagerTest, test_rerouting_service_on_query) // Enable rerouting service, which conducts on query (not status change) node->declare_parameter( - "operations", rclcpp::ParameterValue(std::vector{"ReroutingService"})); + "operations", rclcpp::ParameterValue(std::vector{"rerouting_service"})); nav2::declare_parameter_if_not_declared( - node, "ReroutingService.plugin", + node, "rerouting_service.plugin", rclcpp::ParameterValue(std::string{"nav2_route::ReroutingService"})); std::shared_ptr costmap_subscriber; OperationsManager manager(node, costmap_subscriber); @@ -205,7 +205,7 @@ TEST(OperationsManagerTest, test_rerouting_service_on_query) auto srv_client = nav2::ServiceClient( - "route_server/ReroutingService/reroute", node_int); + "route_server/rerouting_service/reroute", node_int); auto req = std::make_shared(); auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); EXPECT_TRUE(resp->success); @@ -233,9 +233,9 @@ TEST(OperationsManagerTest, test_trigger_event_on_graph) // This tests the trigger event plugin, ON_GRAPH actions in the // Operations Manager as well as the route operations client. node->declare_parameter( - "operations", rclcpp::ParameterValue(std::vector{"OpenDoor"})); + "operations", rclcpp::ParameterValue(std::vector{"open_door"})); nav2::declare_parameter_if_not_declared( - node, "OpenDoor.plugin", + node, "open_door.plugin", rclcpp::ParameterValue(std::string{"nav2_route::TriggerEvent"})); std::shared_ptr costmap_subscriber; OperationsManager manager(node, costmap_subscriber); @@ -256,10 +256,10 @@ TEST(OperationsManagerTest, test_trigger_event_on_graph) op.type = "test"; op.trigger = OperationTrigger::NODE; - op2.type = "OpenDoor"; + op2.type = "open_door"; op2.trigger = OperationTrigger::NODE; - op3.type = "OpenDoor"; + op3.type = "open_door"; op3.trigger = OperationTrigger::NODE; std::string service_name = "open_door"; std::string key = "service_name"; @@ -313,12 +313,12 @@ TEST(OperationsManagerTest, test_trigger_event_on_graph_global_service) // Set the global service to use instead of file settings for conflict testing node->declare_parameter( - "operations", rclcpp::ParameterValue(std::vector{"OpenDoor"})); + "operations", rclcpp::ParameterValue(std::vector{"open_door"})); nav2::declare_parameter_if_not_declared( - node, "OpenDoor.plugin", + node, "open_door.plugin", rclcpp::ParameterValue(std::string{"nav2_route::TriggerEvent"})); nav2::declare_parameter_if_not_declared( - node, "OpenDoor.service_name", + node, "open_door.service_name", rclcpp::ParameterValue(std::string{"hello_world"})); std::shared_ptr costmap_subscriber; OperationsManager manager(node, costmap_subscriber); @@ -336,7 +336,7 @@ TEST(OperationsManagerTest, test_trigger_event_on_graph_global_service) // Setup working case Operation op3; - op3.type = "OpenDoor"; + op3.type = "open_door"; op3.trigger = OperationTrigger::NODE; std::string service_name = "open_door"; std::string key = "service_name"; @@ -367,7 +367,7 @@ TEST(OperationsManagerTest, test_trigger_event_on_graph_global_service) // Now, let's reset without the metadata and see that the global version is now called node2.operations.clear(); Operation op4; - op4.type = "OpenDoor"; + op4.type = "open_door"; op4.trigger = OperationTrigger::NODE; node2.operations.push_back(op4); @@ -401,9 +401,9 @@ TEST(OperationsManagerTest, test_trigger_event_on_graph_failures) // Enable trigger event operation, which conducts on node or edge change // when a graph object contains the request for opening a door only node->declare_parameter( - "operations", rclcpp::ParameterValue(std::vector{"OpenDoor"})); + "operations", rclcpp::ParameterValue(std::vector{"open_door"})); nav2::declare_parameter_if_not_declared( - node, "OpenDoor.plugin", + node, "open_door.plugin", rclcpp::ParameterValue(std::string{"nav2_route::TriggerEvent"})); std::shared_ptr costmap_subscriber; OperationsManager manager(node, costmap_subscriber); @@ -426,7 +426,7 @@ TEST(OperationsManagerTest, test_trigger_event_on_graph_failures) // Setup some test operations Operation op, op2; op.type = "test"; - op2.type = "TriggerEvent"; + op2.type = "trigger_event"; op.trigger = OperationTrigger::NODE; op2.trigger = OperationTrigger::NODE; @@ -446,9 +446,9 @@ TEST(OperationsManagerTest, test_time_marker) auto node = std::make_shared("operations_manager_test"); auto node_thread = std::make_unique(node); node->declare_parameter( - "operations", rclcpp::ParameterValue(std::vector{"TimeMarker"})); + "operations", rclcpp::ParameterValue(std::vector{"time_marker"})); nav2::declare_parameter_if_not_declared( - node, "TimeMarker.plugin", + node, "time_marker.plugin", rclcpp::ParameterValue(std::string{"nav2_route::TimeMarker"})); std::shared_ptr costmap_subscriber; OperationsManager manager(node, costmap_subscriber); @@ -553,7 +553,7 @@ class TestRouteOperations : public nav2_route::RouteOperation { } - std::string getName() override {return "TestRouteOperation";} + std::string getName() override {return "test_route_operation";} OperationResult perform( NodePtr, EdgePtr, diff --git a/nav2_route/test/test_route_server.cpp b/nav2_route/test/test_route_server.cpp index 5fb73b4ce0f..cbad518d725 100644 --- a/nav2_route/test/test_route_server.cpp +++ b/nav2_route/test/test_route_server.cpp @@ -332,7 +332,7 @@ TEST(RouteServerTest, test_complete_action_api) auto node_int = std::make_shared("my_node2"); auto srv_client = nav2::ServiceClient( - "route_server/ReroutingService/reroute", node_int); + "route_server/rerouting_service/reroute", node_int); auto req = std::make_shared(); auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); EXPECT_TRUE(resp->success); diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 98a960cb017..28d447dd21f 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -103,9 +103,9 @@ See inline description of parameters in the `SmacPlanner`. This includes comment ``` planner_server: ros__parameters: - planner_plugins: ["GridBased"] + planner_plugins: ["grid_based"] - GridBased: + grid_based: plugin: "nav2_smac_planner::SmacPlannerHybrid" tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters downsample_costmap: false # whether or not to downsample the map diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 5ebf446be3e..82df518a678 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -193,7 +193,7 @@ class DummySmootherServer : public nav2_smoother::SmootherServer { // Override defaults default_ids_.clear(); - default_ids_.resize(1, "SmoothPath"); + default_ids_.resize(1, "smooth_path"); set_parameter(rclcpp::Parameter("smoother_plugins", default_ids_)); default_types_.clear(); default_types_.resize(1, "DummySmoother"); @@ -241,9 +241,9 @@ class SmootherTest : public ::testing::Test smoother_server_->set_parameter( rclcpp::Parameter( "smoother_plugins", - rclcpp::ParameterValue(std::vector(1, "DummySmoothPath")))); + rclcpp::ParameterValue(std::vector(1, "dummy_smooth_path")))); smoother_server_->declare_parameter( - "DummySmoothPath.plugin", + "dummy_smooth_path.plugin", rclcpp::ParameterValue(std::string("DummySmoother"))); smoother_server_->configure(); smoother_server_->activate(); @@ -330,7 +330,7 @@ class SmootherTest : public ::testing::Test TEST_F(SmootherTest, testingSuccess) { - ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 500ms, true)); + ASSERT_TRUE(sendGoal("dummy_smooth_path", 0.0, 0.0, 1.0, 0.0, 500ms, true)); auto result = getResult(); EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); EXPECT_EQ(result.result->path.poses.size(), (std::size_t)3); @@ -340,7 +340,7 @@ TEST_F(SmootherTest, testingSuccess) TEST_F(SmootherTest, testingFailureOnInvalidSmootherId) { - ASSERT_TRUE(sendGoal("InvalidSmoother", 0.0, 0.0, 1.0, 0.0, 500ms, true)); + ASSERT_TRUE(sendGoal("invalid_smoother", 0.0, 0.0, 1.0, 0.0, 500ms, true)); auto result = getResult(); EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); SUCCEED(); @@ -356,7 +356,7 @@ TEST_F(SmootherTest, testingSuccessOnEmptyPlugin) TEST_F(SmootherTest, testingIncomplete) { - ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 50ms, true)); + ASSERT_TRUE(sendGoal("dummy_smooth_path", 0.0, 0.0, 1.0, 0.0, 50ms, true)); auto result = getResult(); EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); EXPECT_FALSE(result.result->was_completed); @@ -365,7 +365,7 @@ TEST_F(SmootherTest, testingIncomplete) TEST_F(SmootherTest, testingFailureOnException) { - ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 0.0, 0.0, 500ms, true)); + ASSERT_TRUE(sendGoal("dummy_smooth_path", 0.0, 0.0, 0.0, 0.0, 500ms, true)); auto result = getResult(); EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); SUCCEED(); @@ -373,7 +373,7 @@ TEST_F(SmootherTest, testingFailureOnException) TEST_F(SmootherTest, testingFailureOnCollision) { - ASSERT_TRUE(sendGoal("DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, true)); + ASSERT_TRUE(sendGoal("dummy_smooth_path", -4.0, 0.0, 0.0, 0.0, 500ms, true)); auto result = getResult(); EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); SUCCEED(); @@ -381,7 +381,7 @@ TEST_F(SmootherTest, testingFailureOnCollision) TEST_F(SmootherTest, testingCollisionCheckDisabled) { - ASSERT_TRUE(sendGoal("DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, false)); + ASSERT_TRUE(sendGoal("dummy_smooth_path", -4.0, 0.0, 0.0, 0.0, 500ms, false)); auto result = getResult(); EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); SUCCEED(); @@ -393,9 +393,9 @@ TEST(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) smoother_server->set_parameter( rclcpp::Parameter( "smoother_plugins", - rclcpp::ParameterValue(std::vector(1, "DummySmoothPath")))); + rclcpp::ParameterValue(std::vector(1, "dummy_smooth_path")))); smoother_server->declare_parameter( - "DummySmoothPath.plugin", + "dummy_smooth_path.plugin", rclcpp::ParameterValue(std::string("DummySmoother"))); auto state = smoother_server->configure(); EXPECT_EQ(state.id(), 2); // 1 on failure, 2 on success @@ -408,9 +408,9 @@ TEST(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) smoother_server->set_parameter( rclcpp::Parameter( "smoother_plugins", - rclcpp::ParameterValue(std::vector(1, "DummySmoothPath")))); + rclcpp::ParameterValue(std::vector(1, "dummy_smooth_path")))); smoother_server->declare_parameter( - "DummySmoothPath.plugin", + "dummy_smooth_path.plugin", rclcpp::ParameterValue(std::string("InvalidSmootherPlugin"))); auto state = smoother_server->configure(); EXPECT_EQ(state.id(), 1); // 1 on failure, 2 on success diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 7261ab628f8..b1088015931 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -60,8 +60,8 @@ controller_server: min_theta_velocity_threshold: 0.001 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["goal_checker"] - path_handler_plugins: ["PathHandler"] - controller_plugins: ["FollowPath"] + path_handler_plugins: ["path_handler"] + controller_plugins: ["follow_path"] # Progress checker parameters progress_checker: @@ -75,7 +75,7 @@ controller_server: yaw_goal_tolerance: 0.25 path_length_tolerance: 1.0 stateful: true - PathHandler: + path_handler: plugin: "nav2_controller::FeasiblePathHandler" prune_distance: 1.0 enforce_path_inversion: false @@ -85,7 +85,7 @@ controller_server: minimum_rotation_angle: 0.785 reject_unit_path: false # DWB parameters - FollowPath: + follow_path: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: true min_vel_x: 0.0 @@ -231,8 +231,8 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] - GridBased: + planner_plugins: ["grid_based"] + grid_based: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false @@ -297,8 +297,8 @@ collision_monitor: stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: + polygons: ["footprint_approach"] + footprint_approach: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index cba49e2245c..e936426d8b7 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -61,8 +61,8 @@ controller_server: speed_limit_topic: "/speed_limit" progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["goal_checker"] - path_handler_plugins: ["PathHandler"] - controller_plugins: ["FollowPath"] + path_handler_plugins: ["path_handler"] + controller_plugins: ["follow_path"] # Progress checker parameters progress_checker: @@ -76,7 +76,7 @@ controller_server: yaw_goal_tolerance: 0.25 path_length_tolerance: 1.0 stateful: true - PathHandler: + path_handler: plugin: "nav2_controller::FeasiblePathHandler" prune_distance: 2.0 enforce_path_inversion: false @@ -86,7 +86,7 @@ controller_server: minimum_rotation_angle: 0.785 reject_unit_path: false # DWB parameters - FollowPath: + follow_path: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: true min_vel_x: 0.0 @@ -223,8 +223,8 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] - GridBased: + planner_plugins: ["grid_based"] + grid_based: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false @@ -289,8 +289,8 @@ collision_monitor: stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: + polygons: ["footprint_approach"] + footprint_approach: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index 1a93f7278d9..7dc970a7827 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -61,8 +61,8 @@ controller_server: speed_limit_topic: "/speed_limit" progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["goal_checker"] - path_handler_plugins: ["PathHandler"] - controller_plugins: ["FollowPath"] + path_handler_plugins: ["path_handler"] + controller_plugins: ["follow_path"] # Progress checker parameters progress_checker: @@ -76,7 +76,7 @@ controller_server: yaw_goal_tolerance: 0.25 path_length_tolerance: 1.0 stateful: true - PathHandler: + path_handler: plugin: "nav2_controller::FeasiblePathHandler" prune_distance: 2.0 enforce_path_inversion: false @@ -86,7 +86,7 @@ controller_server: minimum_rotation_angle: 0.785 reject_unit_path: false # DWB parameters - FollowPath: + follow_path: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: true min_vel_x: 0.0 @@ -223,8 +223,8 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] - GridBased: + planner_plugins: ["grid_based"] + grid_based: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false @@ -289,8 +289,8 @@ collision_monitor: stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: + polygons: ["footprint_approach"] + footprint_approach: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index 5f2d082ec56..08f589b9c9e 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -56,7 +56,7 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file param_substitutions = { - 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), + 'planner_server.ros__parameters.grid_based.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, 'map_server.ros__parameters.yaml_filename': map_yaml_file, } diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 8aca0e71e28..0dda8e139ca 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -55,7 +55,7 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file param_substitutions = { - 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), + 'planner_server.ros__parameters.grid_based.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, 'map_server.ros__parameters.yaml_filename': map_yaml_file, } diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 2dca76b1a52..ea62c5d6c74 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -7,7 +7,7 @@ controller_server: failure_tolerance: -0.1 progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - path_handler_plugins: ["PathHandler"] + path_handler_plugins: ["path_handler"] controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded","failed_to_make_progress", "no_valid_control"] unknown: plugin: "nav2_system_tests::UnknownErrorController" @@ -40,7 +40,7 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 path_length_tolerance: 1.0 - PathHandler: + path_handler: plugin: "nav2_controller::FeasiblePathHandler" prune_distance: 2.0 enforce_path_inversion: false @@ -50,7 +50,7 @@ controller_server: minimum_rotation_angle: 0.785 reject_unit_path: false # DWB parameters - FollowPath: + follow_path: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: true min_vel_x: 0.0 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 c1fca8802dc..07d13b05f37 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 @@ -34,8 +34,8 @@ controller_server: failure_tolerance: 0.3 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - path_handler_plugins: ["PathHandler"] - controller_plugins: ["FollowPath"] + path_handler_plugins: ["path_handler"] + controller_plugins: ["follow_path"] # Progress checker parameters progress_checker: @@ -54,7 +54,7 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 path_length_tolerance: 1.0 - PathHandler: + path_handler: plugin: "nav2_controller::FeasiblePathHandler" prune_distance: 2.0 enforce_path_inversion: false @@ -64,7 +64,7 @@ controller_server: minimum_rotation_angle: 0.785 reject_unit_path: false # MPPI parameters - FollowPath: + follow_path: plugin: "nav2_mppi_controller::MPPIController" time_steps: 56 model_dt: 0.05 @@ -262,8 +262,8 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] - GridBased: + planner_plugins: ["grid_based"] + grid_based: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false @@ -343,8 +343,8 @@ collision_monitor: stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: + polygons: ["footprint_approach"] + footprint_approach: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 11b163bcfc6..5e343a313c3 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -68,9 +68,9 @@ void PlannerTester::activate() auto state = rclcpp_lifecycle::State(); planner_tester_ = std::make_shared(); planner_tester_->declare_parameter( - "GridBased.use_astar", rclcpp::ParameterValue(true)); + "grid_based.use_astar", rclcpp::ParameterValue(true)); planner_tester_->set_parameter( - rclcpp::Parameter(std::string("GridBased.use_astar"), rclcpp::ParameterValue(true))); + rclcpp::Parameter(std::string("grid_based.use_astar"), rclcpp::ParameterValue(true))); planner_tester_->set_parameter( rclcpp::Parameter(std::string("expected_planner_frequency"), rclcpp::ParameterValue(-1.0))); planner_tester_->set_parameter( diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 20e922bb065..1184f8b2960 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -85,7 +85,7 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer } try { auto dummy_cancel_checker = []() {return false;}; - path = planners_["GridBased"]->createPlan(start, goal, dummy_cancel_checker); + path = planners_["grid_based"]->createPlan(start, goal, dummy_cancel_checker); // The situation when createPlan() did not throw any exception // does not guarantee that plan was created correctly. // So it should be checked additionally that path is correct. diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index f334fc1b89d..4018dcd0d95 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -40,9 +40,9 @@ void testSmallPathValidityAndOrientation(std::string plugin, double length) { auto obj = std::make_shared(); rclcpp_lifecycle::State state; - obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); + obj->set_parameter(rclcpp::Parameter("grid_based.plugin", plugin)); obj->declare_parameter( - "GridBased.use_final_approach_orientation", rclcpp::ParameterValue(false)); + "grid_based.use_final_approach_orientation", rclcpp::ParameterValue(false)); obj->onConfigure(state); geometry_msgs::msg::PoseStamped start; @@ -62,7 +62,7 @@ void testSmallPathValidityAndOrientation(std::string plugin, double length) // Test without use_final_approach_orientation // expecting end path pose orientation to be equal to goal orientation - auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); + auto path = obj->getPlan(start, goal, "grid_based", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01); obj->onCleanup(state); @@ -73,14 +73,14 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) { auto obj = std::make_shared(); rclcpp_lifecycle::State state; - obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); + obj->set_parameter(rclcpp::Parameter("grid_based.plugin", plugin)); // Test WITH use_final_approach_orientation // expecting end path pose orientation to be equal to approach orientation // which in the one pose corner case should be the start pose orientation obj->declare_parameter( - "GridBased.use_final_approach_orientation", rclcpp::ParameterValue(true)); - obj->set_parameter(rclcpp::Parameter("GridBased.use_final_approach_orientation", true)); + "grid_based.use_final_approach_orientation", rclcpp::ParameterValue(true)); + obj->set_parameter(rclcpp::Parameter("grid_based.use_final_approach_orientation", true)); obj->onConfigure(state); geometry_msgs::msg::PoseStamped start; @@ -98,7 +98,7 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) auto dummy_cancel_checker = []() {return false;}; - auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); + auto path = obj->getPlan(start, goal, "grid_based", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); int path_size = path.poses.size(); @@ -123,8 +123,8 @@ void testCancel(std::string plugin) { auto obj = std::make_shared(); rclcpp_lifecycle::State state; - obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); - obj->declare_parameter("GridBased.terminal_checking_interval", rclcpp::ParameterValue(1)); + obj->set_parameter(rclcpp::Parameter("grid_based.plugin", plugin)); + obj->declare_parameter("grid_based.terminal_checking_interval", rclcpp::ParameterValue(1)); obj->onConfigure(state); geometry_msgs::msg::PoseStamped start; @@ -143,7 +143,7 @@ void testCancel(std::string plugin) auto always_cancelled = []() {return true;}; EXPECT_THROW( - obj->getPlan(start, goal, "GridBased", always_cancelled), + obj->getPlan(start, goal, "grid_based", always_cancelled), nav2_core::PlannerCancelled); obj->onCleanup(state); obj.reset(); diff --git a/nav2_system_tests/src/route/test_route_launch.py b/nav2_system_tests/src/route/test_route_launch.py index 6a454563894..352c44c4de5 100755 --- a/nav2_system_tests/src/route/test_route_launch.py +++ b/nav2_system_tests/src/route/test_route_launch.py @@ -60,10 +60,10 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update({'use_astar': 'True'}) param_substitutions.update( - {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} + {'planner_server.ros__parameters.grid_based.plugin': os.getenv('PLANNER', '')} ) param_substitutions.update( - {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} + {'controller_server.ros__parameters.follow_path.plugin': os.getenv('CONTROLLER', '')} ) param_substitutions.update( {'route_server.ros__parameters.max_planning_time': '0.0001'} diff --git a/nav2_system_tests/src/route/tester_node.py b/nav2_system_tests/src/route/tester_node.py index 892f9369277..beffb9d0019 100755 --- a/nav2_system_tests/src/route/tester_node.py +++ b/nav2_system_tests/src/route/tester_node.py @@ -196,7 +196,7 @@ def runTrackRouteTest(self) -> bool: time.sleep(1) self.info_msg('Triggering a reroute') srv_client: Client[Trigger.Request, Trigger.Response] = \ - self.create_client(Trigger, 'route_server/ReroutingService/reroute') + self.create_client(Trigger, 'route_server/rerouting_service/reroute') while not srv_client.wait_for_service(timeout_sec=1.0): self.info_msg('Reroute service not available, waiting...') req = Trigger.Request() diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 954ec7388c0..b32ca07f569 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -77,8 +77,8 @@ controller_server: failure_tolerance: 0.3 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - path_handler_plugins: ["PathHandler"] - controller_plugins: ["FollowPath"] + path_handler_plugins: ["path_handler"] + controller_plugins: ["follow_path"] use_realtime_priority: false # Progress checker parameters @@ -98,7 +98,7 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 path_length_tolerance: 1.0 - PathHandler: + path_handler: plugin: "nav2_controller::FeasiblePathHandler" prune_distance: 2.0 enforce_path_inversion: false @@ -108,7 +108,7 @@ controller_server: minimum_rotation_angle: 0.785 reject_unit_path: false # DWB parameters - FollowPath: + follow_path: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: true min_vel_x: 0.0 @@ -253,8 +253,8 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] - GridBased: + planner_plugins: ["grid_based"] + grid_based: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false @@ -334,8 +334,8 @@ collision_monitor: stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: + polygons: ["footprint_approach"] + footprint_approach: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py index ba11a3e1ac3..c3b3e549169 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py @@ -295,7 +295,7 @@ def run_all_tests(robot_tester: NavTester) -> bool: goal_pose=pose_out_of_bounds, behavior_tree='', expected_error_code=304, - expected_error_msg=('GridBasedplugin failed to plan from ' + expected_error_msg=('grid_based plugin failed to plan from ' '(-2.00, -0.50) to (2000.00, 4000.00): ' '"Goal Coordinates of(2000.000000, 4000.000000) ' 'was outside bounds"')) @@ -333,7 +333,7 @@ def run_all_tests(robot_tester: NavTester) -> bool: # Limit controller to generate very slow velocities # Note assumes nav2_dwb_controller dwb_core::DWBLocalPlanner robot_tester.setControllerParam( - 'FollowPath.max_vel_x', + 'follow_path.max_vel_x', Parameter.Type.DOUBLE, 0.0001) result = robot_tester.runNavigateAction( diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index a1b13bcbaef..ba11a083f19 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -65,10 +65,10 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update({'enable_groot_monitoring': 'True'}) param_substitutions.update( - {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} + {'planner_server.ros__parameters.grid_based.plugin': os.getenv('PLANNER', '')} ) param_substitutions.update( - {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} + {'controller_server.ros__parameters.follow_path.plugin': os.getenv('CONTROLLER', '')} ) configured_params = RewrittenYaml( diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index 095066ea739..6a8a65bcb93 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -63,10 +63,10 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update({'use_astar': 'True'}) param_substitutions.update( - {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} + {'planner_server.ros__parameters.grid_based.plugin': os.getenv('PLANNER', '')} ) param_substitutions.update( - {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} + {'controller_server.ros__parameters.follow_path.plugin': os.getenv('CONTROLLER', '')} ) configured_params = RewrittenYaml( diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index bef573bca71..7f6ddef7a5e 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -65,10 +65,10 @@ def generate_launch_description() -> LaunchDescription: param_substitutions.update({'enable_groot_monitoring': 'True'}) param_substitutions.update( - {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} + {'planner_server.ros__parameters.grid_based.plugin': os.getenv('PLANNER', '')} ) param_substitutions.update( - {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} + {'controller_server.ros__parameters.follow_path.plugin': os.getenv('CONTROLLER', '')} ) configured_params = RewrittenYaml( diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 592efbf007d..f4b04e284d7 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -53,7 +53,7 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file param_substitutions = { - 'planner_server.ros__parameters.GridBased.use_astar': 'False' + 'planner_server.ros__parameters.grid_based.use_astar': 'False' } configured_params = RewrittenYaml( source_file=params_file, diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index eae258b20ff..6fe3e47794e 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -55,8 +55,8 @@ Below are the default values of the parameters : planner_server: ros__parameters: planner_plugin_types: ["nav2_theta_star_planner::ThetaStarPlanner"] - planner_plugin_ids: ["GridBased"] - GridBased: + planner_plugin_ids: ["grid_based"] + grid_based: how_many_corners: 8 w_euc_cost: 1.0 w_traversal_cost: 2.0 diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index a346b8e622a..c4850a6c3d8 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -87,13 +87,13 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) waypoint_task_executor_ = waypoint_task_executor_loader_.createUniqueInstance( waypoint_task_executor_type_); RCLCPP_INFO( - get_logger(), "Created waypoint_task_executor : %s of type %s", + get_logger(), "Created waypoint task executor : %s of type %s", waypoint_task_executor_id_.c_str(), waypoint_task_executor_type_.c_str()); waypoint_task_executor_->initialize(node, waypoint_task_executor_id_); } catch (const std::exception & e) { RCLCPP_FATAL( get_logger(), - "Failed to create waypoint_task_executor. Exception: %s", e.what()); + "Failed to create waypoint task executor. Exception: %s", e.what()); on_cleanup(state); } diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp index 9f8af9f8f53..b3f4b18a42d 100644 --- a/nav2_waypoint_follower/test/test_task_executors.cpp +++ b/nav2_waypoint_follower/test/test_task_executors.cpp @@ -31,12 +31,12 @@ TEST(WaypointFollowerTest, WaitAtWaypoint) { auto node = std::make_shared("testWaypointNode"); - node->declare_parameter("WAW.waypoint_pause_duration", 50); + node->declare_parameter("waw.waypoint_pause_duration", 50); std::unique_ptr waw( new nav2_waypoint_follower::WaitAtWaypoint ); - waw->initialize(node, std::string("WAW")); + waw->initialize(node, std::string("waw")); auto start_time = node->now(); @@ -49,8 +49,8 @@ TEST(WaypointFollowerTest, WaitAtWaypoint) EXPECT_NEAR((end_time - start_time).seconds(), 0.05, 0.01); waw.reset(new nav2_waypoint_follower::WaitAtWaypoint); - node->set_parameter(rclcpp::Parameter("WAW.enabled", false)); - waw->initialize(node, std::string("WAW")); + node->set_parameter(rclcpp::Parameter("waw.enabled", false)); + waw->initialize(node, std::string("waw")); // plugin is not enabled, should exit EXPECT_TRUE(waw->processAtWaypoint(pose, 0)); @@ -74,7 +74,7 @@ TEST(WaypointFollowerTest, InputAtWaypoint) std::unique_ptr iaw( new nav2_waypoint_follower::InputAtWaypoint ); - iaw->initialize(node, std::string("IAW")); + iaw->initialize(node, std::string("iaw")); executor.add_node(node->shared_from_this()->get_node_base_interface()); auto start_time = node->now(); @@ -93,8 +93,8 @@ TEST(WaypointFollowerTest, InputAtWaypoint) t1.join(); iaw.reset(new nav2_waypoint_follower::InputAtWaypoint); - node->set_parameter(rclcpp::Parameter("IAW.enabled", false)); - iaw->initialize(node, std::string("IAW")); + node->set_parameter(rclcpp::Parameter("iaw.enabled", false)); + iaw->initialize(node, std::string("iaw")); // plugin is not enabled, should exit EXPECT_TRUE(iaw->processAtWaypoint(pose, 0)); @@ -137,7 +137,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) std::unique_ptr paw( new nav2_waypoint_follower::PhotoAtWaypoint ); - paw->initialize(node, std::string("PAW")); + paw->initialize(node, std::string("raw")); executor.add_node(node->shared_from_this()->get_node_base_interface()); // no images, throws because can't write @@ -151,8 +151,8 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) t1.join(); paw.reset(new nav2_waypoint_follower::PhotoAtWaypoint); - node->set_parameter(rclcpp::Parameter("PAW.enabled", false)); - paw->initialize(node, std::string("PAW")); + node->set_parameter(rclcpp::Parameter("raw.enabled", false)); + paw->initialize(node, std::string("raw")); // plugin is not enabled, should exit EXPECT_TRUE(paw->processAtWaypoint(pose, 0)); diff --git a/tools/planner_benchmarking/README.md b/tools/planner_benchmarking/README.md index 977d217436e..b1476484667 100644 --- a/tools/planner_benchmarking/README.md +++ b/tools/planner_benchmarking/README.md @@ -8,16 +8,16 @@ To use, modify the Nav2 bringup parameters to include the planners of interest: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["SmacHybrid", "Smac2d", "SmacLattice", "Navfn", "ThetaStar"] - SmacHybrid: + planner_plugins: ["smac_hybrid", "smac_2d", "smac_lattice", "navfn", "theta_star"] + smac_hybrid: plugin: "nav2_smac_planner::SmacPlannerHybrid" - Smac2d: + smac_2d: plugin: "nav2_smac_planner::SmacPlanner2D" - SmacLattice: + smac_lattice: plugin: "nav2_smac_planner::SmacPlannerLattice" - Navfn: + navfn: plugin: "nav2_navfn_planner::NavfnPlanner" - ThetaStar: + theta_star: plugin: "nav2_theta_star_planner::ThetaStarPlanner" ``` diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 443a998c3f9..7a94bf2f4d5 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -114,7 +114,7 @@ def main() -> None: costmap = np.asarray(costmap_msg.data) costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) - planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] + planners = ['navfn', 'theta_star', 'smac_hybrid', 'smac_2d', 'smac_lattice'] max_cost = 210 side_buffer = 100 time_stamp = navigator.get_clock().now().to_msg() diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md index 0be09114eff..f3f0328b2d6 100644 --- a/tools/smoother_benchmarking/README.md +++ b/tools/smoother_benchmarking/README.md @@ -16,8 +16,8 @@ To use the suite, modify the Nav2 bringup parameters `nav2_params.yaml` to inclu planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["SmacHybrid"] - SmacHybrid: + planner_plugins: ["smac_hybrid"] + smac_hybrid: plugin: "nav2_smac_planner::SmacPlannerHybrid" tolerance: 0.5 motion_model_for_search: "DUBIN" # default, non-reverse motion diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py index 45b9f5750de..83525825b52 100644 --- a/tools/smoother_benchmarking/metrics.py +++ b/tools/smoother_benchmarking/metrics.py @@ -126,7 +126,7 @@ def main() -> None: costmap = np.asarray(costmap_msg.data) costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) - planner = 'SmacHybrid' + planner = 'smac_hybrid' smoothers = ['simple_smoother', 'constrained_smoother', 'sg_smoother'] max_cost = 210 side_buffer = 10