diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index 91e3e241b91..f505733588b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -24,7 +24,7 @@ #include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav_msgs/msg/path.hpp" namespace BT @@ -105,19 +105,19 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) } /** - * @brief Parse XML string to std::vector + * @brief Parse XML string to nav2_msgs::msg::PoseStampedArray * @param key XML string - * @return std::vector + * @return nav2_msgs::msg::PoseStampedArray */ template<> -inline std::vector convertFromString(const StringView key) +inline nav2_msgs::msg::PoseStampedArray convertFromString(const StringView key) { // 9 real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if (parts.size() % 9 != 0) { - throw std::runtime_error("invalid number of fields for std::vector attribute)"); + throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)"); } else { - std::vector poses; + nav2_msgs::msg::PoseStampedArray pose_stamped_array; for (size_t i = 0; i < parts.size(); i += 9) { geometry_msgs::msg::PoseStamped pose_stamped; pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[i])); @@ -129,9 +129,9 @@ inline std::vector convertFromString(const Stri pose_stamped.pose.orientation.y = BT::convertFromString(parts[i + 6]); pose_stamped.pose.orientation.z = BT::convertFromString(parts[i + 7]); pose_stamped.pose.orientation.w = BT::convertFromString(parts[i + 8]); - poses.push_back(pose_stamped); + pose_stamped_array.poses.push_back(pose_stamped); } - return poses; + return pose_stamped_array; } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index fe5d2e9dd25..7781de4d3b4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -75,7 +75,7 @@ class ComputePathThroughPosesAction { return providedBasicPorts( { - BT::InputPort>( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 44ad055ea24..2b2676c9392 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -18,8 +18,7 @@ #include #include -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/quaternion.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -74,7 +73,7 @@ class NavigateThroughPosesAction : public BtActionNode>( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp index 8992b4d516a..9e3e634498a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/get_costs.hpp" @@ -30,8 +30,6 @@ namespace nav2_behavior_tree class RemoveInCollisionGoals : public BtServiceNode { public: - typedef std::vector Goals; - /** * @brief A constructor for nav2_behavior_tree::RemoveInCollisionGoals * @param service_node_name Service name this node creates a client for @@ -54,7 +52,8 @@ class RemoveInCollisionGoals : public BtServiceNode { return providedBasicPorts( { - BT::InputPort("input_goals", "Original goals to remove from"), + BT::InputPort("input_goals", + "Original goals to remove from"), BT::InputPort( "cost_threshold", 254.0, "Cost threshold for considering a goal in collision"), @@ -62,7 +61,8 @@ class RemoveInCollisionGoals : public BtServiceNode BT::InputPort( "consider_unknown_as_obstacle", false, "Whether to consider unknown cost as obstacle"), - BT::OutputPort("output_goals", "Goals with in-collision goals removed"), + BT::OutputPort("output_goals", + "Goals with in-collision goals removed"), }); } @@ -70,7 +70,7 @@ class RemoveInCollisionGoals : public BtServiceNode bool use_footprint_; bool consider_unknown_as_obstacle_; double cost_threshold_; - Goals input_goals_; + nav2_msgs::msg::PoseStampedArray input_goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index fcfade6d17b..ab97450de91 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -19,7 +19,7 @@ #include #include -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "behaviortree_cpp/action_node.h" @@ -31,8 +31,6 @@ namespace nav2_behavior_tree class RemovePassedGoals : public BT::ActionNodeBase { public: - typedef std::vector Goals; - RemovePassedGoals( const std::string & xml_tag_name, const BT::NodeConfiguration & conf); @@ -45,8 +43,10 @@ class RemovePassedGoals : public BT::ActionNodeBase static BT::PortsList providedPorts() { return { - BT::InputPort("input_goals", "Original goals to remove viapoints from"), - BT::OutputPort("output_goals", "Goals with passed viapoints removed"), + BT::InputPort("input_goals", + "Original goals to remove viapoints from"), + BT::OutputPort("output_goals", + "Goals with passed viapoints removed"), BT::InputPort("radius", 0.5, "radius to goal for it to be considered for removal"), BT::InputPort("global_frame", "Global frame"), BT::InputPort("robot_base_frame", "Robot base frame"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 12344a5d3f7..a884f84c805 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -59,7 +59,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort>( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -70,7 +70,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode bool first_time; rclcpp::Node::SharedPtr node_; geometry_msgs::msg::PoseStamped goal_; - std::vector goals_; + nav2_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 22893e33ee3..b609abb5b30 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -19,7 +19,7 @@ #include #include "behaviortree_cpp/condition_node.h" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree @@ -56,7 +56,7 @@ class GoalUpdatedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort>( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -65,7 +65,7 @@ class GoalUpdatedCondition : public BT::ConditionNode private: geometry_msgs::msg::PoseStamped goal_; - std::vector goals_; + nav2_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index 63a7f606558..6c469b4760b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -50,7 +50,7 @@ class GoalUpdatedController : public BT::DecoratorNode static BT::PortsList providedPorts() { return { - BT::InputPort>( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -66,7 +66,7 @@ class GoalUpdatedController : public BT::DecoratorNode bool goal_was_updated_; geometry_msgs::msg::PoseStamped goal_; - std::vector goals_; + nav2_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 59a9fc55210..0664280f7b2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -58,7 +58,7 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), - BT::InputPort>( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -120,7 +120,7 @@ class SpeedController : public BT::DecoratorNode // current goal geometry_msgs::msg::PoseStamped goal_; - std::vector goals_; + nav2_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index d6f9ee662d0..074eed93309 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -39,7 +39,7 @@ void RemoveInCollisionGoals::on_tick() getInput("input_goals", input_goals_); getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_); - if (input_goals_.empty()) { + if (input_goals_.poses.empty()) { setOutput("output_goals", input_goals_); should_send_request_ = false; return; @@ -47,24 +47,24 @@ void RemoveInCollisionGoals::on_tick() request_ = std::make_shared(); request_->use_footprint = use_footprint_; - for (const auto & goal : input_goals_) { - request_->poses.push_back(goal); + for (const auto & goal : input_goals_.poses) { + request_->poses.poses.push_back(goal); } } BT::NodeStatus RemoveInCollisionGoals::on_completion( std::shared_ptr response) { - Goals valid_goal_poses; + nav2_msgs::msg::PoseStampedArray valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || response->costs[i] < cost_threshold_) { - valid_goal_poses.push_back(input_goals_[i]); + valid_goal_poses.poses.push_back(input_goals_.poses[i]); } } // Inform if all goals have been removed - if (valid_goal_poses.empty()) { + if (valid_goal_poses.poses.empty()) { RCLCPP_INFO( node_->get_logger(), "All goals are in collision and have been removed from the list"); diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 723e704e437..d027533df07 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -49,10 +49,10 @@ inline BT::NodeStatus RemovePassedGoals::tick() initialize(); } - Goals goal_poses; + nav2_msgs::msg::PoseStampedArray goal_poses; getInput("input_goals", goal_poses); - if (goal_poses.empty()) { + if (goal_poses.poses.empty()) { setOutput("output_goals", goal_poses); return BT::NodeStatus::SUCCESS; } @@ -61,21 +61,21 @@ inline BT::NodeStatus RemovePassedGoals::tick() geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( - current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_, + current_pose, *tf_, goal_poses.poses[0].header.frame_id, robot_base_frame_, transform_tolerance_)) { return BT::NodeStatus::FAILURE; } double dist_to_goal; - while (goal_poses.size() > 1) { - dist_to_goal = euclidean_distance(goal_poses[0].pose, current_pose.pose); + while (goal_poses.poses.size() > 1) { + dist_to_goal = euclidean_distance(goal_poses.poses[0].pose, current_pose.pose); if (dist_to_goal > viapoint_achieved_radius_) { break; } - goal_poses.erase(goal_poses.begin()); + goal_poses.poses.erase(goal_poses.poses.begin()); } setOutput("output_goals", goal_poses); diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index 1e1e557e5f3..cc71e8007e2 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -38,7 +38,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() return BT::NodeStatus::SUCCESS; } - std::vector current_goals; + nav2_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index 962eef70dd2..2e312912eaa 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -33,7 +33,7 @@ BT::NodeStatus GoalUpdatedCondition::tick() return BT::NodeStatus::FAILURE; } - std::vector current_goals; + nav2_msgs::msg::PoseStampedArray current_goals; geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goals", current_goals); BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index b2f235dbd1f..5dbd5f6cb86 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -44,7 +44,7 @@ BT::NodeStatus GoalUpdatedController::tick() setStatus(BT::NodeStatus::RUNNING); - std::vector current_goals; + nav2_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index f5b4eb3bd86..61898dd2227 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -66,7 +66,7 @@ inline BT::NodeStatus SpeedController::tick() first_tick_ = true; } - std::vector current_goals; + nav2_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); 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 02e7c865d8d..2c4a208ead2 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 @@ -44,7 +44,7 @@ class ComputePathThroughPosesActionServer const auto goal = goal_handle->get_goal(); auto result = std::make_shared(); result->path.poses.resize(2); - result->path.poses[1].pose.position.x = goal->goals[0].pose.position.x; + result->path.poses[1].pose.position.x = goal->goals.poses[0].pose.position.x; if (goal->use_start) { result->path.poses[0].pose.position.x = goal->start.pose.position.x; } else { @@ -137,9 +137,9 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - std::vector goals; - goals.resize(1); - goals[0].pose.position.x = 1.0; + nav2_msgs::msg::PoseStampedArray goals; + goals.poses.resize(1); + goals.poses[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); // tick until node succeeds @@ -150,7 +150,7 @@ 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(action_server_->getCurrentGoal()->goals[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, 1.0); EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); @@ -166,7 +166,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal - goals[0].pose.position.x = -2.5; + goals.poses[0].pose.position.x = -2.5; config_->blackboard->set("goals", goals); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { @@ -174,7 +174,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, -2.5); EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); @@ -202,9 +202,9 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) config_->blackboard->set("start", start); // create new goal and set it on blackboard - std::vector goals; - goals.resize(1); - goals[0].pose.position.x = 1.0; + nav2_msgs::msg::PoseStampedArray goals; + goals.poses.resize(1); + goals.poses[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); // tick until node succeeds @@ -215,7 +215,7 @@ 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(action_server_->getCurrentGoal()->goals[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[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")); @@ -232,7 +232,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start - goals[0].pose.position.x = -2.5; + goals.poses[0].pose.position.x = -2.5; start.pose.position.x = -1.5; config_->blackboard->set("goals", goals); config_->blackboard->set("start", start); @@ -242,7 +242,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, -2.5); EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); diff --git a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp index b296d706dc1..a2311eaf471 100644 --- a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp @@ -21,7 +21,7 @@ #include #include "nav_msgs/msg/path.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "behaviortree_cpp/bt_factory.h" @@ -97,11 +97,11 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) // create new path and set it on blackboard nav_msgs::msg::Path path; - std::vector goals; - goals.resize(2); - goals[0].pose.position.x = 1.0; - goals[1].pose.position.x = 2.0; - path.poses = goals; + nav2_msgs::msg::PoseStampedArray goals; + goals.poses.resize(2); + goals.poses[0].pose.position.x = 1.0; + goals.poses[1].pose.position.x = 2.0; + path.poses = goals.poses; path.header.frame_id = "test_frame_1"; config_->blackboard->set("path", path); diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index 9ffd637d66c..bfa1ffac707 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -74,7 +74,7 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test "wait_for_service_timeout", std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); - std::vector poses; + nav2_msgs::msg::PoseStampedArray poses; config_->blackboard->set( "goals", poses); @@ -132,10 +132,10 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - std::vector poses; - poses.resize(1); - poses[0].pose.position.x = -2.5; - poses[0].pose.orientation.x = 1.0; + nav2_msgs::msg::PoseStampedArray poses; + poses.poses.resize(1); + poses.poses[0].pose.position.x = -2.5; + poses.poses[0].pose.orientation.x = 1.0; config_->blackboard->set("goals", poses); // tick until node succeeds diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp index dbb4889298d..b91300f7985 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -125,19 +125,19 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - std::vector poses; - poses.resize(4); - poses[0].pose.position.x = 0.0; - poses[0].pose.position.y = 0.0; + nav2_msgs::msg::PoseStampedArray poses; + poses.poses.resize(4); + poses.poses[0].pose.position.x = 0.0; + poses.poses[0].pose.position.y = 0.0; - poses[1].pose.position.x = 0.5; - poses[1].pose.position.y = 0.0; + poses.poses[1].pose.position.x = 0.5; + poses.poses[1].pose.position.y = 0.0; - poses[2].pose.position.x = 1.0; - poses[2].pose.position.y = 0.0; + poses.poses[2].pose.position.x = 1.0; + poses.poses[2].pose.position.y = 0.0; - poses[3].pose.position.x = 2.0; - poses[3].pose.position.y = 0.0; + poses.poses[3].pose.position.x = 2.0; + poses.poses[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -147,13 +147,13 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) } // check that it removed the point in range - std::vector output_poses; + nav2_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.size(), 3u); - EXPECT_EQ(output_poses[0], poses[0]); - EXPECT_EQ(output_poses[1], poses[1]); - EXPECT_EQ(output_poses[2], poses[2]); + EXPECT_EQ(output_poses.poses.size(), 3u); + EXPECT_EQ(output_poses.poses[0], poses.poses[0]); + EXPECT_EQ(output_poses.poses[1], poses.poses[1]); + EXPECT_EQ(output_poses.poses[2], poses.poses[2]); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 1ddc5c45f3c..5a07ac1659d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -114,19 +114,19 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - std::vector poses; - poses.resize(4); - poses[0].pose.position.x = 0.0; - poses[0].pose.position.y = 0.0; + nav2_msgs::msg::PoseStampedArray poses; + poses.poses.resize(4); + poses.poses[0].pose.position.x = 0.0; + poses.poses[0].pose.position.y = 0.0; - poses[1].pose.position.x = 0.5; - poses[1].pose.position.y = 0.0; + poses.poses[1].pose.position.x = 0.5; + poses.poses[1].pose.position.y = 0.0; - poses[2].pose.position.x = 1.0; - poses[2].pose.position.y = 0.0; + poses.poses[2].pose.position.x = 1.0; + poses.poses[2].pose.position.y = 0.0; - poses[3].pose.position.x = 2.0; - poses[3].pose.position.y = 0.0; + poses.poses[3].pose.position.x = 2.0; + poses.poses[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -136,12 +136,12 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) } // check that it removed the point in range - std::vector output_poses; + nav2_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.size(), 2u); - EXPECT_EQ(output_poses[0], poses[2]); - EXPECT_EQ(output_poses[1], poses[3]); + EXPECT_EQ(output_poses.poses.size(), 2u); + EXPECT_EQ(output_poses.poses[0], poses.poses[2]); + EXPECT_EQ(output_poses.poses[1], poses.poses[3]); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp index 4259e87a53e..7aa211f32a1 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -32,8 +32,8 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree // Setting fake goals on blackboard geometry_msgs::msg::PoseStamped goal1; goal1.header.stamp = node_->now(); - std::vector poses1; - poses1.push_back(goal1); + nav2_msgs::msg::PoseStampedArray poses1; + poses1.poses.push_back(goal1); config_->blackboard->set("goal", goal1); config_->blackboard->set("goals", poses1); bt_node_ = std::make_shared( @@ -63,8 +63,8 @@ TEST_F(GoalUpdatedControllerTestFixture, test_behavior) // Creating updated fake-goals geometry_msgs::msg::PoseStamped goal2; goal2.header.stamp = node_->now(); - std::vector poses2; - poses2.push_back(goal2); + nav2_msgs::msg::PoseStampedArray poses2; + poses2.poses.push_back(goal2); // starting in idle EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 903b6385ed3..76fab537b06 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -42,7 +42,7 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi goal.header.stamp = node_->now(); config_->blackboard->set("goal", goal); - std::vector fake_poses; + nav2_msgs::msg::PoseStampedArray fake_poses; config_->blackboard->set("goals", fake_poses); // NOLINT config_->input_ports["min_rate"] = 0.1; diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 008e0a36b77..4a0a37d97e1 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -194,68 +194,68 @@ TEST(PoseStampedPortTest, test_correct_syntax) EXPECT_EQ(value.pose.orientation.w, 7.0); } -TEST(PoseStampedVectorPortTest, test_wrong_syntax) +TEST(PoseStampedArrayPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>>( - "PoseStampedVectorPortTest"); + factory.registerNodeType>( + "PoseStampedArrayPortTest"); EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( - + )"; EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } -TEST(PoseStampedVectorPortTest, test_correct_syntax) +TEST(PoseStampedArrayPortTest, test_correct_syntax) { std::string xml_txt = R"( - + )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>>( - "PoseStampedVectorPortTest"); + factory.registerNodeType>( + "PoseStampedArrayPortTest"); auto tree = factory.createTreeFromText(xml_txt); tree = factory.createTreeFromText(xml_txt); - std::vector values; + nav2_msgs::msg::PoseStampedArray values; tree.rootNode()->getInput("test", values); - EXPECT_EQ(rclcpp::Time(values[0].header.stamp).nanoseconds(), 0); - EXPECT_EQ(values[0].header.frame_id, "map"); - EXPECT_EQ(values[0].pose.position.x, 1.0); - EXPECT_EQ(values[0].pose.position.y, 2.0); - EXPECT_EQ(values[0].pose.position.z, 3.0); - EXPECT_EQ(values[0].pose.orientation.x, 4.0); - EXPECT_EQ(values[0].pose.orientation.y, 5.0); - EXPECT_EQ(values[0].pose.orientation.z, 6.0); - EXPECT_EQ(values[0].pose.orientation.w, 7.0); - EXPECT_EQ(rclcpp::Time(values[1].header.stamp).nanoseconds(), 0); - EXPECT_EQ(values[1].header.frame_id, "odom"); - EXPECT_EQ(values[1].pose.position.x, 8.0); - EXPECT_EQ(values[1].pose.position.y, 9.0); - EXPECT_EQ(values[1].pose.position.z, 10.0); - EXPECT_EQ(values[1].pose.orientation.x, 11.0); - EXPECT_EQ(values[1].pose.orientation.y, 12.0); - EXPECT_EQ(values[1].pose.orientation.z, 13.0); - EXPECT_EQ(values[1].pose.orientation.w, 14.0); + EXPECT_EQ(rclcpp::Time(values.poses[0].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values.poses[0].header.frame_id, "map"); + EXPECT_EQ(values.poses[0].pose.position.x, 1.0); + EXPECT_EQ(values.poses[0].pose.position.y, 2.0); + EXPECT_EQ(values.poses[0].pose.position.z, 3.0); + EXPECT_EQ(values.poses[0].pose.orientation.x, 4.0); + EXPECT_EQ(values.poses[0].pose.orientation.y, 5.0); + EXPECT_EQ(values.poses[0].pose.orientation.z, 6.0); + EXPECT_EQ(values.poses[0].pose.orientation.w, 7.0); + EXPECT_EQ(rclcpp::Time(values.poses[1].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values.poses[1].header.frame_id, "odom"); + EXPECT_EQ(values.poses[1].pose.position.x, 8.0); + EXPECT_EQ(values.poses[1].pose.position.y, 9.0); + EXPECT_EQ(values.poses[1].pose.position.z, 10.0); + EXPECT_EQ(values.poses[1].pose.orientation.x, 11.0); + EXPECT_EQ(values.poses[1].pose.orientation.y, 12.0); + EXPECT_EQ(values.poses[1].pose.orientation.z, 13.0); + EXPECT_EQ(values.poses[1].pose.orientation.w, 14.0); } TEST(PathPortTest, test_wrong_syntax) diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index aee07de14c9..c13a5137d09 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav_msgs/msg/path.hpp" @@ -40,8 +41,6 @@ class NavigateThroughPosesNavigator { public: using ActionT = nav2_msgs::action::NavigateThroughPoses; - typedef std::vector Goals; - /** * @brief A constructor for NavigateThroughPosesNavigator */ diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 98d86499800..d2da0a3615b 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -102,10 +102,10 @@ NavigateThroughPosesNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); - Goals goal_poses; + nav2_msgs::msg::PoseStampedArray goal_poses; [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses); - if (goal_poses.size() == 0) { + if (goal_poses.poses.size() == 0) { bt_action_server_->publishFeedback(feedback_msg); return; } @@ -173,7 +173,7 @@ NavigateThroughPosesNavigator::onLoop() feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; - feedback_msg->number_of_poses_remaining = goal_poses.size(); + feedback_msg->number_of_poses_remaining = goal_poses.poses.size(); bt_action_server_->publishFeedback(feedback_msg); } @@ -212,8 +212,8 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) bool NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal) { - Goals goal_poses = goal->poses; - for (auto & goal_pose : goal_poses) { + nav2_msgs::msg::PoseStampedArray pose_stamped_array = goal->poses; + for (auto & goal_pose : pose_stamped_array) { if (!nav2_util::transformPoseInTargetFrame( goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, feedback_utils_.transform_tolerance)) @@ -226,10 +226,11 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr } } - if (goal_poses.size() > 0) { + if (pose_stamped_array.poses.size() > 0) { RCLCPP_INFO( logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)", - goal_poses.size(), goal_poses.back().pose.position.x, goal_poses.back().pose.position.y); + pose_stamped_array.poses.size(), pose_stamped_array.poses.back().pose.position.x, + pose_stamped_array.poses.back().pose.position.y); } // Reset state for new action feedback @@ -238,7 +239,8 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goals_blackboard_id_, std::move(goal_poses)); + blackboard->set(goals_blackboard_id_, + std::move(pose_stamped_array)); return true; } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5413fd3d2c1..a9f27a00bf0 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -826,7 +826,7 @@ void Costmap2DROS::getCostsCallback( Costmap2D * costmap = layered_costmap_->getCostmap(); - for (const auto & pose : request->poses) { + for (const auto & pose : request->poses.poses) { geometry_msgs::msg::PoseStamped pose_transformed; transformPoseToGlobalFrame(pose, pose_transformed); bool in_bounds = costmap->worldToMap( diff --git a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp index 15780196b17..118479343a7 100644 --- a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp @@ -53,7 +53,7 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = 0.5; pose.pose.position.y = 1.0; - request->poses.push_back(pose); + request->poses.poses.push_back(pose); request->use_footprint = false; auto result_future = client_->async_send_request(request); @@ -78,7 +78,7 @@ TEST_F(GetCostServiceTest, TestWithFootprint) tf2::Quaternion q; q.setRPY(0, 0, 0.5); pose.pose.orientation = tf2::toMsg(q); - request->poses.push_back(pose); + request->poses.poses.push_back(pose); request->use_footprint = true; auto result_future = client_->async_send_request(request); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index a9f93b5ecbc..fbb6997083d 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" + "msg/PoseStampedArray.msg" "srv/GetCosts.srv" "srv/GetCostmap.srv" "srv/IsPathValid.srv" diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index bedd9396797..962c0c2c9b8 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,5 +1,5 @@ #goal definition -geometry_msgs/PoseStamped[] goals +nav2_msgs/PoseStampedArray goals geometry_msgs/PoseStamped start string planner_id bool use_start # If false, use current robot pose as path start, if true, use start above instead diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index 9b5d8af05f4..33185936532 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -1,7 +1,7 @@ #goal definition uint32 number_of_loops uint32 goal_index 0 -geometry_msgs/PoseStamped[] poses +nav2_msgs/PoseStampedArray poses --- #result definition diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 898e4540ef7..a7f145b746e 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,6 +1,6 @@ #goal definition -geometry_msgs/PoseStamped[] poses +nav2_msgs/PoseStampedArray poses string behavior_tree --- #result definition diff --git a/nav2_msgs/msg/PoseStampedArray.msg b/nav2_msgs/msg/PoseStampedArray.msg new file mode 100644 index 00000000000..1a4e04dc0d1 --- /dev/null +++ b/nav2_msgs/msg/PoseStampedArray.msg @@ -0,0 +1,4 @@ +# An array of stamped poses with a header for global reference. + +std_msgs/Header header +geometry_msgs/PoseStamped[] poses \ No newline at end of file diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index 55ebad4f0a2..43009661906 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -1,6 +1,6 @@ # Get costmap costs at given poses bool use_footprint -geometry_msgs/PoseStamped[] poses +nav2_msgs/PoseStampedArray poses --- float32[] costs \ No newline at end of file