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 @@ -16,6 +16,7 @@
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_

#include <string>
#include <vector>

#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
Expand Down Expand Up @@ -74,7 +75,8 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
{
return providedBasicPorts(
{
BT::InputPort<geometry_msgs::msg::PoseStamped>("goals", "Destinations to plan through"),
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
"goals", "Destinations to plan through"),
BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
BT::OutputPort<ActionResult::_error_code_type>(
"error_code_id", "The navigate through poses error code"),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,14 +34,14 @@ class AreErrorCodesPresent : public BT::ConditionNode
const BT::NodeConfiguration & conf)
: BT::ConditionNode(condition_name, conf)
{
getInput<std::set<unsigned short>>("error_codes_to_check", error_codes_to_check_); //NOLINT
getInput<std::set<uint16_t>>("error_codes_to_check", error_codes_to_check_); //NOLINT
}

AreErrorCodesPresent() = delete;

BT::NodeStatus tick()
{
getInput<unsigned short>("error_code", error_code_); //NOLINT
getInput<uint16_t>("error_code", error_code_); //NOLINT

if (error_codes_to_check_.find(error_code_) != error_codes_to_check_.end()) {
return BT::NodeStatus::SUCCESS;
Expand All @@ -54,14 +54,14 @@ class AreErrorCodesPresent : public BT::ConditionNode
{
return
{
BT::InputPort<unsigned short>("error_code", "The active error codes"), //NOLINT
BT::InputPort<std::set<unsigned short>>("error_codes_to_check", "Error codes to check")//NOLINT
BT::InputPort<uint16_t>("error_code", "The active error codes"), //NOLINT
BT::InputPort<std::set<uint16_t>>("error_codes_to_check", "Error codes to check")//NOLINT
};
}

protected:
unsigned short error_code_; //NOLINT
std::set<unsigned short> error_codes_to_check_; //NOLINT
uint16_t error_code_; //NOLINT
std::set<uint16_t> error_codes_to_check_; //NOLINT
};

} // namespace nav2_behavior_tree
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF
using ActionGoal = Action::Goal;
void SetUp()
{
int error_code = ActionGoal::NONE;
std::set<unsigned short> error_codes_to_check = {ActionGoal::UNKNOWN}; //NOLINT
uint16_t error_code = ActionGoal::NONE;
std::set<uint16_t> error_codes_to_check = {ActionGoal::UNKNOWN}; //NOLINT
config_->blackboard->set("error_code", error_code);
config_->blackboard->set("error_codes_to_check", error_codes_to_check);

Expand Down Expand Up @@ -58,7 +58,7 @@ std::shared_ptr<BT::Tree> AreErrorCodesPresentFixture::tree_ = nullptr;

TEST_F(AreErrorCodesPresentFixture, test_condition)
{
std::map<int, BT::NodeStatus> error_to_status_map = {
std::map<uint16_t, BT::NodeStatus> error_to_status_map = {
{ActionGoal::NONE, BT::NodeStatus::FAILURE},
{ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class WouldAControllerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorT
using ActionGoal = Action::Goal;
void SetUp()
{
int error_code = ActionGoal::NONE;
uint16_t error_code = ActionGoal::NONE;
config_->blackboard->set("error_code", error_code);

std::string xml_txt =
Expand Down Expand Up @@ -56,7 +56,7 @@ std::shared_ptr<BT::Tree> WouldAControllerRecoveryHelpFixture::tree_ = nullptr;

TEST_F(WouldAControllerRecoveryHelpFixture, test_condition)
{
std::map<int, BT::NodeStatus> error_to_status_map = {
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},
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class WouldAPlannerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTree
using ActionGoal = Action::Goal;
void SetUp()
{
int error_code = ActionGoal::NONE;
uint16_t error_code = ActionGoal::NONE;
config_->blackboard->set("error_code", error_code);

std::string xml_txt =
Expand Down Expand Up @@ -56,7 +56,7 @@ std::shared_ptr<BT::Tree> WouldAPlannerRecoveryHelpFixture::tree_ = nullptr;

TEST_F(WouldAPlannerRecoveryHelpFixture, test_condition)
{
std::map<int, BT::NodeStatus> error_to_status_map = {
std::map<uint16_t, BT::NodeStatus> error_to_status_map = {
{ActionGoal::NONE, BT::NodeStatus::FAILURE},
{ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
{ActionGoal::NO_VALID_PATH, BT::NodeStatus::SUCCESS},
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class WouldASmootherRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTre
using ActionGoal = Action::Goal;
void SetUp()
{
int error_code = ActionGoal::NONE;
uint16_t error_code = ActionGoal::NONE;
config_->blackboard->set("error_code", error_code);

std::string xml_txt =
Expand Down Expand Up @@ -56,7 +56,7 @@ std::shared_ptr<BT::Tree> WouldASmootherRecoveryHelpFixture::tree_ = nullptr;

TEST_F(WouldASmootherRecoveryHelpFixture, test_condition)
{
std::map<int, BT::NodeStatus> error_to_status_map = {
std::map<uint16_t, BT::NodeStatus> error_to_status_map = {
{ActionGoal::NONE, BT::NodeStatus::FAILURE},
{ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
{ActionGoal::TIMEOUT, BT::NodeStatus::SUCCESS},
Expand Down
2 changes: 1 addition & 1 deletion nav2_mppi_controller/test/utils_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -368,7 +368,7 @@ TEST(UtilsTests, SmootherTest)

// Check that path is smoother
float smoothed_val, original_val;
for (unsigned int i = 0; i != noisey_sequence.vx.shape(0); i++) {
for (unsigned int i = 1; i != noisey_sequence.vx.shape(0) - 1; i++) {
smoothed_val += fabs(noisey_sequence.vx(i) - 0.2);
smoothed_val += fabs(noisey_sequence.vy(i) - 0.0);
smoothed_val += fabs(noisey_sequence.wz(i) - 0.3);
Expand Down