Skip to content

Commit

Permalink
rename param
Browse files Browse the repository at this point in the history
Signed-off-by: David Brown <[email protected]>
  • Loading branch information
david-wb committed Dec 5, 2024
1 parent 4de9265 commit 3ea2011
Show file tree
Hide file tree
Showing 12 changed files with 26 additions and 26 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
BT::InputPort<bool>("no_collision_checks", false, "Disable collision checking"),
BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
BT::OutputPort<ActionResult::_error_code_type>(
"error_code_id", "The back up behavior server error code")
});
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
BT::InputPort<double>("dist_to_travel", 0.15, "Distance to travel"),
BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
BT::InputPort<bool>("no_collision_checks", false, "Disable collision checking"),
BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
BT::OutputPort<Action::Result::_error_code_type>(
"error_code_id", "The drive on heading behavior server error code")
});
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 @@ -13,7 +13,7 @@
<input_port name="time_allowance">Allowed time for reversing</input_port>
<input_port name="server_name">Server name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
<input_port name="no_collision_checks">Disable collision checking</input_port>
<input_port name="disable_collision_checks">Disable collision checking</input_port>
<output_port name="error_code_id">"Back up error code"</output_port>
</Action>

Expand All @@ -23,7 +23,7 @@
<input_port name="time_allowance">Allowed time for reversing</input_port>
<input_port name="server_name">Server name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
<input_port name="no_collision_checks">Disable collision checking</input_port>
<input_port name="disable_collision_checks">Disable collision checking</input_port>
<output_port name="error_code_id">"Drive on heading error code"</output_port>
</Action>

Expand Down
6 changes: 3 additions & 3 deletions nav2_behavior_tree/plugins/action/back_up_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,16 +36,16 @@ void nav2_behavior_tree::BackUpAction::initialize()
getInput("backup_speed", speed);
double time_allowance;
getInput("time_allowance", time_allowance);
double no_collision_checks;
getInput("no_collision_checks", no_collision_checks);
double disable_collision_checks;
getInput("disable_collision_checks", disable_collision_checks);

// Populate the input message
goal_.target.x = dist;
goal_.target.y = 0.0;
goal_.target.z = 0.0;
goal_.speed = speed;
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
goal_.no_collision_checks = no_collision_checks;
goal_.disable_collision_checks = disable_collision_checks;
}

void BackUpAction::on_tick()
Expand Down
6 changes: 3 additions & 3 deletions nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,16 +37,16 @@ void DriveOnHeadingAction::initialize()
getInput("speed", speed);
double time_allowance;
getInput("time_allowance", time_allowance);
double no_collision_checks;
getInput("no_collision_checks", no_collision_checks);
double disable_collision_checks;
getInput("disable_collision_checks", disable_collision_checks);

// Populate the input message
goal_.target.x = dist;
goal_.target.y = 0.0;
goal_.target.z = 0.0;
goal_.speed = speed;
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
goal_.no_collision_checks = no_collision_checks;
goal_.disable_collision_checks = disable_collision_checks;
initalized_ = true;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -131,20 +131,20 @@ TEST_F(BackUpActionTestFixture, test_ports)
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), 0.15);
EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_speed"), 0.025);
EXPECT_EQ(tree_->rootNode()->getInput<bool>("no_collision_checks"), false);
EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), false);

xml_txt =
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<BackUp backup_dist="2" backup_speed="0.26" no_collision_checks="true" />
<BackUp backup_dist="2" backup_speed="0.26" disable_collision_checks="true" />
</BehaviorTree>
</root>)";

tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), 2.0);
EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_speed"), 0.26);
EXPECT_EQ(tree_->rootNode()->getInput<bool>("no_collision_checks"), true);
EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), true);
}

TEST_F(BackUpActionTestFixture, test_tick)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -128,20 +128,20 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports)
EXPECT_EQ(tree_->rootNode()->getInput<double>("dist_to_travel"), 0.15);
EXPECT_EQ(tree_->rootNode()->getInput<double>("speed"), 0.025);
EXPECT_EQ(tree_->rootNode()->getInput<double>("time_allowance"), 10.0);
EXPECT_EQ(tree_->rootNode()->getInput<bool>("no_collision_checks"), false);
EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), false);

xml_txt =
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<DriveOnHeading dist_to_travel="2" speed="0.26" no_collision_checks="true" />
<DriveOnHeading dist_to_travel="2" speed="0.26" disable_collision_checks="true" />
</BehaviorTree>
</root>)";

tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
EXPECT_EQ(tree_->rootNode()->getInput<double>("dist_to_travel"), 2.0);
EXPECT_EQ(tree_->rootNode()->getInput<double>("speed"), 0.26);
EXPECT_EQ(tree_->rootNode()->getInput<bool>("no_collision_checks"), true);
EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), true);
}

TEST_F(DriveOnHeadingActionTestFixture, test_tick)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
feedback_(std::make_shared<typename ActionT::Feedback>()),
command_x_(0.0),
command_speed_(0.0),
command_no_collision_checks_(false),
command_disable_collision_checks_(false),
simulate_ahead_time_(0.0)
{
}
Expand Down Expand Up @@ -169,7 +169,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
const geometry_msgs::msg::Twist & cmd_vel,
geometry_msgs::msg::Pose2D & pose2d)
{
if (command_no_collision_checks_) {
if (command_disable_collision_checks_) {
return true;
}

Expand Down Expand Up @@ -220,7 +220,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
geometry_msgs::msg::PoseStamped initial_pose_;
double command_x_;
double command_speed_;
bool command_no_collision_checks_;
bool command_disable_collision_checks_;
rclcpp::Duration command_time_allowance_{0, 0};
rclcpp::Time end_time_;
double simulate_ahead_time_;
Expand Down
2 changes: 1 addition & 1 deletion nav2_msgs/action/BackUp.action
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
geometry_msgs/Point target
float32 speed
builtin_interfaces/Duration time_allowance
bool no_collision_checks
bool disable_collision_checks
---
#result definition

Expand Down
2 changes: 1 addition & 1 deletion nav2_msgs/action/DriveOnHeading.action
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
geometry_msgs/Point target
float32 speed
builtin_interfaces/Duration time_allowance
bool no_collision_checks
bool disable_collision_checks
---
#result definition

Expand Down
2 changes: 1 addition & 1 deletion nav2_simple_commander/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ New as of September 2023: the simple navigator constructor will accept a `namesp
| followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. |
| followPath(path, controller_id='', goal_checker_id='') | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. |
| spin(spin_dist=1.57, time_allowance=10) | Requests the robot to performs an in-place rotation by a given angle. |
| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10, no_collision_checks=False) | Requests the robot to back up by a given distance. |
| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10, disable_collision_checks=False) | Requests the robot to back up by a given distance. |
| cancelTask() | Cancel an ongoing task request.|
| isTaskComplete() | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. |
| getFeedback() | Gets feedback from task, returns action server feedback object. |
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -279,15 +279,15 @@ def spin(self, spin_dist=1.57, time_allowance=10):
return True

def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10,
no_collision_checks=False):
disable_collision_checks=False):
self.debug("Waiting for 'Backup' action server")
while not self.backup_client.wait_for_server(timeout_sec=1.0):
self.info("'Backup' action server not available, waiting...")
goal_msg = BackUp.Goal()
goal_msg.target = Point(x=float(backup_dist))
goal_msg.speed = backup_speed
goal_msg.time_allowance = Duration(sec=time_allowance)
goal_msg.no_collision_checks = no_collision_checks
goal_msg.disable_collision_checks = disable_collision_checks

self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....')
send_goal_future = self.backup_client.send_goal_async(
Expand All @@ -304,15 +304,15 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10,
return True

def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10,
no_collision_checks=False):
disable_collision_checks=False):
self.debug("Waiting for 'DriveOnHeading' action server")
while not self.backup_client.wait_for_server(timeout_sec=1.0):
self.info("'DriveOnHeading' action server not available, waiting...")
goal_msg = DriveOnHeading.Goal()
goal_msg.target = Point(x=float(dist))
goal_msg.speed = speed
goal_msg.time_allowance = Duration(sec=time_allowance)
goal_msg.no_collision_checks = no_collision_checks
goal_msg.disable_collision_checks = disable_collision_checks

self.info(f'Drive {goal_msg.target.x} m on heading at {goal_msg.speed} m/s....')
send_goal_future = self.drive_on_heading_client.send_goal_async(
Expand Down

0 comments on commit 3ea2011

Please sign in to comment.