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 0591f100b3b..0de7da7925c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -23,8 +23,9 @@ #include "rclcpp/node.hpp" #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 "geometry_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/quaternion.hpp" #include "nav_msgs/msg/path.hpp" namespace BT @@ -112,7 +113,6 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) template<> inline std::vector 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)"); @@ -135,6 +135,38 @@ inline std::vector convertFromString(const Stri } } +/** + * @brief Parse XML string to geometry_msgs::msg::PoseStampedArray + * @param key XML string + * @return geometry_msgs::msg::PoseStampedArray + */ +template<> +inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView key) +{ + auto parts = BT::splitString(key, ';'); + if ((parts.size() - 2) % 9 != 0) { + throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)"); + } else { + geometry_msgs::msg::PoseStampedArray pose_stamped_array; + pose_stamped_array.header.stamp = rclcpp::Time(BT::convertFromString(parts[0])); + pose_stamped_array.header.frame_id = BT::convertFromString(parts[1]); + for (size_t i = 2; i < parts.size(); i += 9) { + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[i])); + pose_stamped.header.frame_id = BT::convertFromString(parts[i + 1]); + pose_stamped.pose.position.x = BT::convertFromString(parts[i + 2]); + pose_stamped.pose.position.y = BT::convertFromString(parts[i + 3]); + pose_stamped.pose.position.z = BT::convertFromString(parts[i + 4]); + pose_stamped.pose.orientation.x = BT::convertFromString(parts[i + 5]); + 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]); + pose_stamped_array.poses.push_back(pose_stamped); + } + return pose_stamped_array; + } +} + /** * @brief Parse XML string to nav_msgs::msg::Path * @param key XML string @@ -143,7 +175,6 @@ inline std::vector convertFromString(const Stri template<> inline nav_msgs::msg::Path convertFromString(const StringView key) { - // 9 real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if ((parts.size() - 2) % 9 != 0) { throw std::runtime_error("invalid number of fields for Path attribute)"); 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..6a74de1bced 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..ea24baadae1 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 "geometry_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..5ed0e213ea7 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 "geometry_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_; + geometry_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..77c2fc367e9 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 "geometry_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..5e730017d92 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 "geometry_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_; + geometry_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..4ea81d896b2 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 "geometry_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_; + geometry_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..49215ae9a14 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_; + geometry_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..3d1dcff2962 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_; + geometry_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 8728cd1cd86..76b50ac3f51 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,7 +47,7 @@ void RemoveInCollisionGoals::on_tick() request_ = std::make_shared(); request_->use_footprint = use_footprint_; - for (const auto & goal : input_goals_) { + for (const auto & goal : input_goals_.poses) { request_->poses.push_back(goal); } } @@ -63,16 +63,16 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion( return BT::NodeStatus::FAILURE; } - Goals valid_goal_poses; + geometry_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..9b44a0635ef 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; + geometry_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..fcd3a665134 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; + geometry_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..06ddf76ae95 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; + geometry_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..d78c5d1e1fd 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; + geometry_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..347f1ccd25c 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; + geometry_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..bb813205456 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; + geometry_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; + geometry_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..3596af739c3 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 "geometry_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; + geometry_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..cf0c1079068 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; + geometry_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; + geometry_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 a9fd10dfc1f..307bd6e317d 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 @@ -147,19 +147,19 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su 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; + geometry_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); @@ -171,13 +171,13 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); // check that it removed the point in range - std::vector output_poses; + geometry_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]); } TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail) @@ -194,19 +194,19 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa 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; + geometry_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); @@ -218,14 +218,14 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa // check that it failed and returned the original goals EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); - std::vector output_poses; + geometry_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.size(), 4u); - 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[3], poses[3]); + EXPECT_EQ(output_poses.poses.size(), 4u); + 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]); + EXPECT_EQ(output_poses.poses[3], poses.poses[3]); } 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..9ed40d8bad7 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; + geometry_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; + geometry_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..d1c3c5e4207 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); + geometry_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); + geometry_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..ae07ee3c21b 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; + geometry_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..3d3ab54aba9 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -258,6 +258,70 @@ TEST(PoseStampedVectorPortTest, test_correct_syntax) EXPECT_EQ(values[1].pose.orientation.w, 14.0); } +TEST(PoseStampedArrayPortTest, test_wrong_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>( + "PoseStampedArrayPortTest"); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + + xml_txt = + R"( + + + + + )"; + + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); +} + +TEST(PoseStampedArrayPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>( + "PoseStampedArrayPortTest"); + auto tree = factory.createTreeFromText(xml_txt); + + tree = factory.createTreeFromText(xml_txt); + geometry_msgs::msg::PoseStampedArray values; + tree.rootNode()->getInput("test", values); + 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) { std::string xml_txt = 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..38f7228c7b5 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 "geometry_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..58f66b1fba8 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; + geometry_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) { + geometry_msgs::msg::PoseStampedArray pose_stamped_array = goal->poses; + for (auto & goal_pose : pose_stamped_array.poses) { 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_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index bedd9396797..0e07373c338 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,5 +1,5 @@ #goal definition -geometry_msgs/PoseStamped[] goals +geometry_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/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 898e4540ef7..9b2896398f6 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,6 +1,6 @@ #goal definition -geometry_msgs/PoseStamped[] poses +geometry_msgs/PoseStampedArray poses string behavior_tree --- #result definition diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6359721e348..c227851f07b 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -394,7 +394,7 @@ void PlannerServer::computePlanThroughPoses() getPreemptedGoalIfRequested(action_server_poses_, goal); - if (goal->goals.empty()) { + if (goal->goals.poses.empty()) { throw nav2_core::NoViapointsGiven("No viapoints given"); } @@ -409,7 +409,7 @@ void PlannerServer::computePlanThroughPoses() }; // Get consecutive paths through these points - for (unsigned int i = 0; i != goal->goals.size(); i++) { + for (unsigned int i = 0; i != goal->goals.poses.size(); i++) { // Get starting point if (i == 0) { curr_start = start; @@ -419,7 +419,7 @@ void PlannerServer::computePlanThroughPoses() curr_start = concat_path.poses.back(); curr_start.header = concat_path.header; } - curr_goal = goal->goals[i]; + curr_goal = goal->goals.poses[i]; // Transform them into the global frame if (!transformPosesToGlobalFrame(curr_start, curr_goal)) { diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 471dce5ce5a..3e07ba8c2bc 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -102,7 +102,7 @@ private Q_SLOTS: std::vector orientation); void startWaypointFollowing(std::vector poses); void startNavigation(geometry_msgs::msg::PoseStamped); - void startNavThroughPoses(std::vector poses); + void startNavThroughPoses(geometry_msgs::msg::PoseStampedArray poses); using NavigationGoalHandle = rclcpp_action::ClientGoalHandle; using WaypointFollowerGoalHandle = @@ -196,8 +196,8 @@ private Q_SLOTS: QState * accumulated_wp_{nullptr}; QState * accumulated_nav_through_poses_{nullptr}; - std::vector acummulated_poses_; - std::vector store_poses_; + geometry_msgs::msg::PoseStampedArray acummulated_poses_; + geometry_msgs::msg::PoseStampedArray store_poses_; // Publish the visual markers with the waypoints void updateWpNavigationMarkers(); diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index e10f1eccdb4..36b74a3dfd8 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -675,7 +675,7 @@ void Nav2Panel::loophandler() void Nav2Panel::handleGoalLoader() { - acummulated_poses_.clear(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); std::cout << "Loading Waypoints!" << std::endl; @@ -699,7 +699,7 @@ void Nav2Panel::handleGoalLoader() auto waypoint = waypoint_iter[it->first.as()]; auto pose = waypoint["pose"].as>(); auto orientation = waypoint["orientation"].as>(); - acummulated_poses_.push_back(convert_to_msg(pose, orientation)); + acummulated_poses_.poses.push_back(convert_to_msg(pose, orientation)); } // Publishing Waypoint Navigation marker after loading wp's @@ -731,7 +731,7 @@ void Nav2Panel::handleGoalSaver() { // Check if the waypoints are accumulated - if (acummulated_poses_.empty()) { + if (acummulated_poses_.poses.empty()) { std::cout << "No accumulated Points to Save!" << std::endl; return; } else { @@ -744,18 +744,19 @@ void Nav2Panel::handleGoalSaver() out << YAML::BeginMap; // Save WPs to data structure - for (unsigned int i = 0; i < acummulated_poses_.size(); ++i) { + for (unsigned int i = 0; i < acummulated_poses_.poses.size(); ++i) { out << YAML::Key << "waypoint" + std::to_string(i); out << YAML::BeginMap; out << YAML::Key << "pose"; std::vector pose = - {acummulated_poses_[i].pose.position.x, acummulated_poses_[i].pose.position.y, - acummulated_poses_[i].pose.position.z}; + {acummulated_poses_.poses[i].pose.position.x, acummulated_poses_.poses[i].pose.position.y, + acummulated_poses_.poses[i].pose.position.z}; out << YAML::Value << pose; out << YAML::Key << "orientation"; std::vector orientation = - {acummulated_poses_[i].pose.orientation.w, acummulated_poses_[i].pose.orientation.x, - acummulated_poses_[i].pose.orientation.y, acummulated_poses_[i].pose.orientation.z}; + {acummulated_poses_.poses[i].pose.orientation.w, acummulated_poses_.poses[i].pose.orientation.x, + acummulated_poses_.poses[i].pose.orientation.y, + acummulated_poses_.poses[i].pose.orientation.z}; out << YAML::Value << orientation; out << YAML::EndMap; } @@ -836,10 +837,10 @@ Nav2Panel::onInitialize() // Clearing all the stored values once reaching the final goal if ( loop_count_ == stoi(nr_of_loops_->displayText().toStdString()) && - goal_index_ == static_cast(store_poses_.size()) - 1 && + goal_index_ == static_cast(store_poses_.poses.size()) - 1 && msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { - store_poses_.clear(); + store_poses_ = geometry_msgs::msg::PoseStampedArray(); waypoint_status_indicator_->clear(); loop_no_ = "0"; loop_count_ = 0; @@ -935,8 +936,8 @@ Nav2Panel::onCancel() &Nav2Panel::onCancelButtonPressed, this)); waypoint_status_indicator_->clear(); - store_poses_.clear(); - acummulated_poses_.clear(); + store_poses_ = geometry_msgs::msg::PoseStampedArray(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); } void Nav2Panel::onResumedWp() @@ -966,12 +967,12 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) pose.pose.position.z = 0.0; pose.pose.orientation = orientationAroundZAxis(theta); - if (store_poses_.empty()) { + if (store_poses_.poses.empty()) { if (state_machine_.configuration().contains(accumulating_)) { waypoint_status_indicator_->clear(); - acummulated_poses_.push_back(pose); + acummulated_poses_.poses.push_back(pose); } else { - acummulated_poses_.clear(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); updateWpNavigationMarkers(); std::cout << "Start navigation" << std::endl; startNavigation(pose); @@ -1030,7 +1031,7 @@ Nav2Panel::onCancelButtonPressed() void Nav2Panel::onAccumulatedWp() { - if (acummulated_poses_.empty()) { + if (acummulated_poses_.poses.empty()) { state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); waypoint_status_indicator_->setText( " Note: Uh oh! Someone forgot to select the waypoints"); @@ -1052,7 +1053,7 @@ Nav2Panel::onAccumulatedWp() /** Making sure that the pose array does not get updated * between the process**/ - if (store_poses_.empty()) { + if (store_poses_.poses.empty()) { std::cout << "Start waypoint" << std::endl; // Setting the final loop value on the text box for sanity @@ -1065,12 +1066,12 @@ Nav2Panel::onAccumulatedWp() if (store_initial_pose_) { try { init_transform = tf2_buffer_->lookupTransform( - acummulated_poses_[0].header.frame_id, base_frame_, + acummulated_poses_.poses[0].header.frame_id, base_frame_, tf2::TimePointZero); } catch (const tf2::TransformException & ex) { RCLCPP_INFO( client_node_->get_logger(), "Could not transform %s to %s: %s", - acummulated_poses_[0].header.frame_id.c_str(), base_frame_.c_str(), ex.what()); + acummulated_poses_.poses[0].header.frame_id.c_str(), base_frame_.c_str(), ex.what()); return; } @@ -1086,24 +1087,24 @@ Nav2Panel::onAccumulatedWp() initial_pose.pose.orientation.w = init_transform.transform.rotation.w; // inserting the acummulated pose - acummulated_poses_.insert(acummulated_poses_.begin(), initial_pose); + acummulated_poses_.poses.insert(acummulated_poses_.poses.begin(), initial_pose); updateWpNavigationMarkers(); initial_pose_stored_ = true; if (loop_count_ == 0) { goal_index_ = 1; } } else if (!store_initial_pose_ && initial_pose_stored_) { - acummulated_poses_.erase( - acummulated_poses_.begin(), - acummulated_poses_.begin()); + acummulated_poses_.poses.erase( + acummulated_poses_.poses.begin(), + acummulated_poses_.poses.begin()); } } else { std::cout << "Resuming waypoint" << std::endl; } - startWaypointFollowing(acummulated_poses_); + startWaypointFollowing(acummulated_poses_.poses); store_poses_ = acummulated_poses_; - acummulated_poses_.clear(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); } void @@ -1116,8 +1117,8 @@ Nav2Panel::onAccumulatedNTP() void Nav2Panel::onAccumulating() { - acummulated_poses_.clear(); - store_poses_.clear(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); + store_poses_ = geometry_msgs::msg::PoseStampedArray(); loop_count_ = 0; loop_no_ = "0"; initial_pose_stored_ = false; @@ -1254,7 +1255,7 @@ Nav2Panel::startWaypointFollowing(std::vector p } void -Nav2Panel::startNavThroughPoses(std::vector poses) +Nav2Panel::startNavThroughPoses(geometry_msgs::msg::PoseStampedArray poses) { auto is_action_server_ready = nav_through_poses_action_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -1272,8 +1273,8 @@ Nav2Panel::startNavThroughPoses(std::vector pos RCLCPP_DEBUG( client_node_->get_logger(), "Sending a path of %zu waypoints:", - nav_through_poses_goal_.poses.size()); - for (auto waypoint : nav_through_poses_goal_.poses) { + nav_through_poses_goal_.poses.poses.size()); + for (auto waypoint : nav_through_poses_goal_.poses.poses) { RCLCPP_DEBUG( client_node_->get_logger(), "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); @@ -1384,14 +1385,14 @@ Nav2Panel::updateWpNavigationMarkers() auto marker_array = std::make_unique(); - for (size_t i = 0; i < acummulated_poses_.size(); i++) { + for (size_t i = 0; i < acummulated_poses_.poses.size(); i++) { // Draw a green arrow at the waypoint pose visualization_msgs::msg::Marker arrow_marker; - arrow_marker.header = acummulated_poses_[i].header; + arrow_marker.header = acummulated_poses_.poses[i].header; arrow_marker.id = getUniqueId(); arrow_marker.type = visualization_msgs::msg::Marker::ARROW; arrow_marker.action = visualization_msgs::msg::Marker::ADD; - arrow_marker.pose = acummulated_poses_[i].pose; + arrow_marker.pose = acummulated_poses_.poses[i].pose; arrow_marker.scale.x = 0.3; arrow_marker.scale.y = 0.05; arrow_marker.scale.z = 0.02; @@ -1405,11 +1406,11 @@ Nav2Panel::updateWpNavigationMarkers() // Draw a red circle at the waypoint pose visualization_msgs::msg::Marker circle_marker; - circle_marker.header = acummulated_poses_[i].header; + circle_marker.header = acummulated_poses_.poses[i].header; circle_marker.id = getUniqueId(); circle_marker.type = visualization_msgs::msg::Marker::SPHERE; circle_marker.action = visualization_msgs::msg::Marker::ADD; - circle_marker.pose = acummulated_poses_[i].pose; + circle_marker.pose = acummulated_poses_.poses[i].pose; circle_marker.scale.x = 0.05; circle_marker.scale.y = 0.05; circle_marker.scale.z = 0.05; @@ -1423,11 +1424,11 @@ Nav2Panel::updateWpNavigationMarkers() // Draw the waypoint number visualization_msgs::msg::Marker marker_text; - marker_text.header = acummulated_poses_[i].header; + marker_text.header = acummulated_poses_.poses[i].header; marker_text.id = getUniqueId(); marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_text.action = visualization_msgs::msg::Marker::ADD; - marker_text.pose = acummulated_poses_[i].pose; + marker_text.pose = acummulated_poses_.poses[i].pose; marker_text.pose.position.z += 0.2; // draw it on top of the waypoint marker_text.scale.x = 0.07; marker_text.scale.y = 0.07; diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 0e59df8814c..079481444e8 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -557,7 +557,9 @@ def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False) goal_msg = ComputePathThroughPoses.Goal() goal_msg.start = start - goal_msg.goals = goals + goal_msg.goals.header.frame_id = 'map' + goal_msg.goals.header.stamp = self.get_clock().now().to_msg() + goal_msg.goals.poses = goals goal_msg.planner_id = planner_id goal_msg.use_start = use_start diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py index 2bcea319313..ba65fccf1bb 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -94,7 +94,9 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateThroughPoses.Goal() - goal_msg.poses = [ + goal_msg.poses.header.frame_id = 'map' + goal_msg.poses.header.stamp = self.get_clock().now().to_msg() + goal_msg.poses.poses = [ self.getStampedPoseMsg(self.goal_pose), self.getStampedPoseMsg(self.goal_pose), ] @@ -164,7 +166,7 @@ def runNavigatePreemptionAction(self, block): ) goal_msg = NavigateThroughPoses.Goal() - goal_msg.poses = [self.getStampedPoseMsg(self.initial_pose)] + goal_msg.poses.poses = [self.getStampedPoseMsg(self.initial_pose)] self.info_msg('Sending goal request...') send_goal_future = self.action_client.send_goal_async(goal_msg) diff --git a/tools/underlay.repos b/tools/underlay.repos index 005f11ec3f0..e443ce12489 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -35,3 +35,8 @@ repositories: type: git url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git version: 091b5ff12436890a54de6325df3573ae110e912b + ros/common_interfaces: + type: git + url: https://github.com/ros2/common_interfaces.git + version: rolling +