Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,6 @@ namespace nav2_behavior_tree
class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTeleop>
{
using Action = nav2_msgs::action::AssistedTeleop;
using ActionGoal = Action::Goal;
using ActionResult = Action::Result;

public:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@ namespace nav2_behavior_tree
class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
{
using Action = nav2_msgs::action::BackUp;
using ActionGoal = Action::Goal;
using ActionResult = Action::Result;

public:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@ class ComputePathThroughPosesAction
{
using Action = nav2_msgs::action::ComputePathThroughPoses;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
{
using Action = nav2_msgs::action::ComputePathToPose;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,7 @@ namespace nav2_behavior_tree
class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeading>
{
using Action = nav2_msgs::action::DriveOnHeading;
using ActionGoal = Action::Goal;
using ActionResult = Action::Goal;
using ActionResult = Action::Result;

public:
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
{
using Action = nav2_msgs::action::FollowPath;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
{
using Action = nav2_msgs::action::NavigateThroughPoses;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@ class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPo
{
using Action = nav2_msgs::action::NavigateToPose;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::acti
{
using Action = nav2_msgs::action::SmoothPath;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,6 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
{
using Action = nav2_msgs::action::Spin;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ namespace nav2_behavior_tree
class WouldAControllerRecoveryHelp : public AreErrorCodesPresent
{
using Action = nav2_msgs::action::FollowPath;
using ActionGoal = Action::Goal;
using ActionResult = Action::Result;

public:
WouldAControllerRecoveryHelp(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ namespace nav2_behavior_tree
class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent
{
using Action = nav2_msgs::action::ComputePathToPose;
using ActionGoal = Action::Goal;
using ActionResult = Action::Result;

public:
WouldAPlannerRecoveryHelp(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ namespace nav2_behavior_tree
class WouldASmootherRecoveryHelp : public AreErrorCodesPresent
{
using Action = nav2_msgs::action::SmoothPath;
using ActionGoal = Action::Goal;
using ActionResult = Action::Result;

public:
WouldASmootherRecoveryHelp(
Expand Down
4 changes: 2 additions & 2 deletions nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void AssistedTeleopAction::on_tick()

BT::NodeStatus AssistedTeleopAction::on_success()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -55,7 +55,7 @@ BT::NodeStatus AssistedTeleopAction::on_aborted()

BT::NodeStatus AssistedTeleopAction::on_cancelled()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
4 changes: 2 additions & 2 deletions nav2_behavior_tree/plugins/action/back_up_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ void BackUpAction::on_tick()

BT::NodeStatus BackUpAction::on_success()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -60,7 +60,7 @@ BT::NodeStatus BackUpAction::on_aborted()

BT::NodeStatus BackUpAction::on_cancelled()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success()
{
setOutput("path", result_.result->path);
// Set empty error code, action was successful
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -59,7 +59,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled()
nav_msgs::msg::Path empty_path;
setOutput("path", empty_path);
// Set empty error code, action was cancelled
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success()
{
setOutput("path", result_.result->path);
// Set empty error code, action was successful
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -58,7 +58,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled()
nav_msgs::msg::Path empty_path;
setOutput("path", empty_path);
// Set empty error code, action was cancelled
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
4 changes: 2 additions & 2 deletions nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ DriveOnHeadingAction::DriveOnHeadingAction(

BT::NodeStatus DriveOnHeadingAction::on_success()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -55,7 +55,7 @@ BT::NodeStatus DriveOnHeadingAction::on_aborted()

BT::NodeStatus DriveOnHeadingAction::on_cancelled()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
4 changes: 2 additions & 2 deletions nav2_behavior_tree/plugins/action/follow_path_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ void FollowPathAction::on_tick()

BT::NodeStatus FollowPathAction::on_success()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -51,7 +51,7 @@ BT::NodeStatus FollowPathAction::on_aborted()
BT::NodeStatus FollowPathAction::on_cancelled()
{
// Set empty error code, action was cancelled
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ void NavigateThroughPosesAction::on_tick()

BT::NodeStatus NavigateThroughPosesAction::on_success()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -54,7 +54,7 @@ BT::NodeStatus NavigateThroughPosesAction::on_aborted()
BT::NodeStatus NavigateThroughPosesAction::on_cancelled()
{
// Set empty error code, action was cancelled
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
4 changes: 2 additions & 2 deletions nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ void NavigateToPoseAction::on_tick()

BT::NodeStatus NavigateToPoseAction::on_success()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -53,7 +53,7 @@ BT::NodeStatus NavigateToPoseAction::on_aborted()
BT::NodeStatus NavigateToPoseAction::on_cancelled()
{
// Set empty error code, action was cancelled
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
4 changes: 2 additions & 2 deletions nav2_behavior_tree/plugins/action/smooth_path_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ BT::NodeStatus SmoothPathAction::on_success()
setOutput("smoothing_duration", rclcpp::Duration(result_.result->smoothing_duration).seconds());
setOutput("was_completed", result_.result->was_completed);
// Set empty error code, action was successful
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -58,7 +58,7 @@ BT::NodeStatus SmoothPathAction::on_aborted()
BT::NodeStatus SmoothPathAction::on_cancelled()
{
// Set empty error code, action was cancelled
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
4 changes: 2 additions & 2 deletions nav2_behavior_tree/plugins/action/spin_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void SpinAction::on_tick()

BT::NodeStatus SpinAction::on_success()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -55,7 +55,7 @@ BT::NodeStatus SpinAction::on_aborted()

BT::NodeStatus SpinAction::on_cancelled()
{
setOutput("error_code_id", ActionGoal::NONE);
setOutput("error_code_id", ActionResult::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,10 @@ WouldAControllerRecoveryHelp::WouldAControllerRecoveryHelp(
: AreErrorCodesPresent(condition_name, conf)
{
error_codes_to_check_ = {
ActionGoal::UNKNOWN,
ActionGoal::PATIENCE_EXCEEDED,
ActionGoal::FAILED_TO_MAKE_PROGRESS,
ActionGoal::NO_VALID_CONTROL
ActionResult::UNKNOWN,
ActionResult::PATIENCE_EXCEEDED,
ActionResult::FAILED_TO_MAKE_PROGRESS,
ActionResult::NO_VALID_CONTROL
};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,9 @@ WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp(
: AreErrorCodesPresent(condition_name, conf)
{
error_codes_to_check_ = {
ActionGoal::UNKNOWN,
ActionGoal::NO_VALID_PATH,
ActionGoal::TIMEOUT
ActionResult::UNKNOWN,
ActionResult::NO_VALID_PATH,
ActionResult::TIMEOUT
};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,10 @@ WouldASmootherRecoveryHelp::WouldASmootherRecoveryHelp(
: AreErrorCodesPresent(condition_name, conf)
{
error_codes_to_check_ = {
ActionGoal::UNKNOWN,
ActionGoal::TIMEOUT,
ActionGoal::FAILED_TO_SMOOTH_PATH,
ActionGoal::SMOOTHED_PATH_IN_COLLISION
ActionResult::UNKNOWN,
ActionResult::TIMEOUT,
ActionResult::FAILED_TO_SMOOTH_PATH,
ActionResult::SMOOTHED_PATH_IN_COLLISION
};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,11 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF
{
public:
using Action = nav2_msgs::action::FollowPath;
using ActionGoal = Action::Goal;
using ActionResult = Action::Result;
void SetUp()
{
uint16_t error_code = ActionGoal::NONE;
std::set<uint16_t> error_codes_to_check = {ActionGoal::UNKNOWN}; //NOLINT
uint16_t error_code = ActionResult::NONE;
std::set<uint16_t> error_codes_to_check = {ActionResult::UNKNOWN}; //NOLINT
config_->blackboard->set("error_code", error_code);
config_->blackboard->set("error_codes_to_check", error_codes_to_check);

Expand Down Expand Up @@ -59,8 +59,8 @@ std::shared_ptr<BT::Tree> AreErrorCodesPresentFixture::tree_ = nullptr;
TEST_F(AreErrorCodesPresentFixture, test_condition)
{
std::map<uint16_t, BT::NodeStatus> error_to_status_map = {
{ActionGoal::NONE, BT::NodeStatus::FAILURE},
{ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
{ActionResult::NONE, BT::NodeStatus::FAILURE},
{ActionResult::UNKNOWN, BT::NodeStatus::SUCCESS},
};

for (const auto & error_to_status : error_to_status_map) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,10 @@ class WouldAControllerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorT
{
public:
using Action = nav2_msgs::action::FollowPath;
using ActionGoal = Action::Goal;
using ActionResult = Action::Result;
void SetUp()
{
uint16_t error_code = ActionGoal::NONE;
uint16_t error_code = ActionResult::NONE;
config_->blackboard->set("error_code", error_code);

std::string xml_txt =
Expand Down Expand Up @@ -57,14 +57,14 @@ std::shared_ptr<BT::Tree> WouldAControllerRecoveryHelpFixture::tree_ = nullptr;
TEST_F(WouldAControllerRecoveryHelpFixture, test_condition)
{
std::map<uint16_t, BT::NodeStatus> error_to_status_map = {
{ActionGoal::NONE, BT::NodeStatus::FAILURE},
{ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
{ActionGoal::INVALID_CONTROLLER, BT::NodeStatus::FAILURE},
{ActionGoal::TF_ERROR, BT::NodeStatus::FAILURE},
{ActionGoal::INVALID_PATH, BT::NodeStatus::FAILURE},
{ActionGoal::PATIENCE_EXCEEDED, BT::NodeStatus::SUCCESS},
{ActionGoal::FAILED_TO_MAKE_PROGRESS, BT::NodeStatus::SUCCESS},
{ActionGoal::NO_VALID_CONTROL, BT::NodeStatus::SUCCESS},
{ActionResult::NONE, BT::NodeStatus::FAILURE},
{ActionResult::UNKNOWN, BT::NodeStatus::SUCCESS},
{ActionResult::INVALID_CONTROLLER, BT::NodeStatus::FAILURE},
{ActionResult::TF_ERROR, BT::NodeStatus::FAILURE},
{ActionResult::INVALID_PATH, BT::NodeStatus::FAILURE},
{ActionResult::PATIENCE_EXCEEDED, BT::NodeStatus::SUCCESS},
{ActionResult::FAILED_TO_MAKE_PROGRESS, BT::NodeStatus::SUCCESS},
{ActionResult::NO_VALID_CONTROL, BT::NodeStatus::SUCCESS},
};

for (const auto & error_to_status : error_to_status_map) {
Expand Down
Loading