Skip to content
Open
2 changes: 1 addition & 1 deletion nav2_behavior_tree/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ Once a new node is registered with the factory, it is now available to the Behav
<BehaviorTree ID="MainTree">
<Sequence name="root">
<ComputePathToPose goal="${goal}"/>
<FollowPath path="${path}" controller_property="FollowPath"/>
<FollowPath path="${path}" controller_id="follow_path"/>
</Sequence>
</BehaviorTree>
</root>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -59,16 +59,16 @@ class GoalCheckerSelector : public BT::SyncActionNode
return {
BT::InputPort<std::string>(
"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<std::string>(
"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<std::string>(
"selected_goal_checker",
"Selected goal_checker by subscription")
"Selected goal checker by subscription")
};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -58,16 +58,16 @@ class ProgressCheckerSelector : public BT::SyncActionNode
return {
BT::InputPort<std::string>(
"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<std::string>(
"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<std::string>(
"selected_progress_checker",
"Selected progress_checker by subscription")
"Selected progress checker by subscription")
};
}

Expand Down
4 changes: 2 additions & 2 deletions nav2_behavior_tree/nav2_tree_nodes.xml
Original file line number Diff line number Diff line change
Expand Up @@ -170,7 +170,7 @@
</Action>

<Action ID="SmoothPath">
<input_port name="smoother_id" default="SmoothPath"/>
<input_port name="smoother_id" default="smooth_path"/>
<input_port name="unsmoothed_path">Path to be smoothed</input_port>
<input_port name="max_smoothing_duration">Maximum smoothing duration</input_port>
<input_port name="check_for_collisions">Bool if collision check should be performed</input_port>
Expand All @@ -182,7 +182,7 @@
</Action>

<Action ID="FollowPath">
<input_port name="controller_id" default="FollowPath"/>
<input_port name="controller_id" default="follow_path"/>
<input_port name="path">Path to follow</input_port>
<input_port name="goal_checker_id">Goal checker</input_port>
<input_port name="progress_checker_id">Progress checker</input_port>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="grid_based"/>
</BehaviorTree>
</root>)";

Expand All @@ -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<std::string>("planner_id"), std::string("GridBased"));
EXPECT_EQ(tree_->rootNode()->getInput<std::string>("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;
Expand Down Expand Up @@ -189,7 +189,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<ComputePathThroughPoses goals="{goals}" start="{start}" path="{path}" planner_id="GridBased"/>
<ComputePathThroughPoses goals="{goals}" start="{start}" path="{path}" planner_id="grid_based"/>
</BehaviorTree>
</root>)";

Expand All @@ -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<std::string>("planner_id"), std::string("GridBased"));
EXPECT_EQ(tree_->rootNode()->getInput<std::string>("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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<ComputePathToPose goal="{goal}" path="{path}" planner_id="grid_based"/>
</BehaviorTree>
</root>)";

Expand All @@ -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<std::string>("planner_id"), std::string("GridBased"));
EXPECT_EQ(tree_->rootNode()->getInput<std::string>("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;
Expand Down Expand Up @@ -186,7 +186,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<ComputePathToPose goal="{goal}" start="{start}" path="{path}" planner_id="GridBased"/>
<ComputePathToPose goal="{goal}" start="{start}" path="{path}" planner_id="grid_based"/>
</BehaviorTree>
</root>)";

Expand All @@ -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<std::string>("planner_id"), std::string("GridBased"));
EXPECT_EQ(tree_->rootNode()->getInput<std::string>("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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<ControllerSelector selected_controller="{selected_controller}" default_controller="DWB" topic_name="controller_selector_custom_topic_name"/>
<ControllerSelector selected_controller="{selected_controller}" default_controller="dwb" topic_name="controller_selector_custom_topic_name"/>
</BehaviorTree>
</root>)";

Expand All @@ -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<std_msgs::msg::String>(
Expand All @@ -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)
Expand All @@ -147,7 +147,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<ControllerSelector selected_controller="{selected_controller}" default_controller="GridBased"/>
<ControllerSelector selected_controller="{selected_controller}" default_controller="follow_path"/>
</BehaviorTree>
</root>)";

Expand All @@ -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<std_msgs::msg::String>(
Expand All @@ -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)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,7 @@ TEST_F(FollowPathActionTestFixture, test_tick)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<FollowPath path="{path}" controller_id="FollowPath"/>
<FollowPath path="{path}" controller_id="follow_path"/>
</BehaviorTree>
</root>)";

Expand All @@ -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<std::string>("controller_id"), std::string("FollowPath"));
EXPECT_EQ(tree_->rootNode()->getInput<std::string>("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();
Expand Down Expand Up @@ -186,7 +186,7 @@ TEST(FollowPathAction, testProgressCheckerIdUpdate)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<FollowPath path="{path}" controller_id="FollowPath" progress_checker_id="{progress_checker_id}"/>
<FollowPath path="{path}" controller_id="follow_path" progress_checker_id="{progress_checker_id}"/>
</BehaviorTree>
</root>)";

Expand Down Expand Up @@ -226,7 +226,7 @@ TEST(FollowPathAction, testGoalCheckerIdUpdate)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<FollowPath path="{path}" controller_id="FollowPath" goal_checker_id="{goal_checker_id}" progress_checker_id="{progress_checker_id}"/>
<FollowPath path="{path}" controller_id="follow_path" goal_checker_id="{goal_checker_id}" progress_checker_id="{progress_checker_id}"/>
</BehaviorTree>
</root>)";

Expand Down Expand Up @@ -266,7 +266,7 @@ TEST(FollowPathAction, testControllerIdUpdate)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<FollowPath path="{path}" controller_id="{controller_id}" goal_checker_id="FollowPath" progress_checker_id="{progress_checker_id}"/>
<FollowPath path="{path}" controller_id="{controller_id}" goal_checker_id="goal_checker" progress_checker_id="{progress_checker_id}"/>
</BehaviorTree>
</root>)";

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="SimpleGoalCheck" topic_name="goal_checker_selector_custom_topic_name"/>
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="simple_goal_checker" topic_name="goal_checker_selector_custom_topic_name"/>
</BehaviorTree>
</root>)";

Expand All @@ -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();

Expand All @@ -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)
Expand All @@ -148,7 +148,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="GridBased"/>
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="goal_checker"/>
</BehaviorTree>
</root>)";

Expand All @@ -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();

Expand All @@ -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)
Expand Down
Loading
Loading