Skip to content

Commit 02336a2

Browse files
tonynajjartonynajjar
authored
Use PoseStampedArray (#4791)
* Add IsStoppedBTNode Signed-off-by: Tony Najjar <[email protected]> * add topic name + reformat Signed-off-by: Tony Najjar <[email protected]> * fix comment Signed-off-by: Tony Najjar <[email protected]> * fix abs Signed-off-by: Tony Najjar <[email protected]> * remove log Signed-off-by: Tony Najjar <[email protected]> * add getter functions for raw twist Signed-off-by: Tony Najjar <[email protected]> * remove unused code Signed-off-by: Tony Najjar <[email protected]> * use odomsmoother Signed-off-by: Tony Najjar <[email protected]> * fix formatting Signed-off-by: Tony Najjar <[email protected]> * update groot Signed-off-by: Tony Najjar <[email protected]> * Add test Signed-off-by: Tony Najjar <[email protected]> * reset at success Signed-off-by: Tony Najjar <[email protected]> * FIX velocity_threshold_ Signed-off-by: Tony Najjar <[email protected]> * Fix stopped Node Signed-off-by: Tony Najjar <[email protected]> * Add tests to odometry_utils Signed-off-by: Tony Najjar <[email protected]> * fix linting Signed-off-by: Tony Najjar <[email protected]> * Use PoseStampedArray Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fixes Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * use geometry_msgs Signed-off-by: Tony Najjar <[email protected]> * more fixes Signed-off-by: Tony Najjar <[email protected]> * . Signed-off-by: Tony Najjar <[email protected]> * revert Signed-off-by: Tony Najjar <[email protected]> * . Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * update Signed-off-by: Tony Najjar <[email protected]> * fix bt_utils Signed-off-by: Tony Najjar <[email protected]> * fix import Signed-off-by: Tony Najjar <[email protected]> * populate Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]>
1 parent 43d7f7e commit 02336a2

33 files changed

+280
-175
lines changed

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp

+34-3
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,9 @@
2323
#include "rclcpp/node.hpp"
2424
#include "behaviortree_cpp/behavior_tree.h"
2525
#include "geometry_msgs/msg/point.hpp"
26-
#include "geometry_msgs/msg/quaternion.hpp"
2726
#include "geometry_msgs/msg/pose_stamped.hpp"
27+
#include "geometry_msgs/msg/pose_stamped_array.hpp"
28+
#include "geometry_msgs/msg/quaternion.hpp"
2829
#include "nav_msgs/msg/path.hpp"
2930

3031
namespace BT
@@ -112,7 +113,6 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
112113
template<>
113114
inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const StringView key)
114115
{
115-
// 9 real numbers separated by semicolons
116116
auto parts = BT::splitString(key, ';');
117117
if (parts.size() % 9 != 0) {
118118
throw std::runtime_error("invalid number of fields for std::vector<PoseStamped> attribute)");
@@ -135,6 +135,38 @@ inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const Stri
135135
}
136136
}
137137

138+
/**
139+
* @brief Parse XML string to geometry_msgs::msg::PoseStampedArray
140+
* @param key XML string
141+
* @return geometry_msgs::msg::PoseStampedArray
142+
*/
143+
template<>
144+
inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView key)
145+
{
146+
auto parts = BT::splitString(key, ';');
147+
if ((parts.size() - 2) % 9 != 0) {
148+
throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)");
149+
} else {
150+
geometry_msgs::msg::PoseStampedArray pose_stamped_array;
151+
pose_stamped_array.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
152+
pose_stamped_array.header.frame_id = BT::convertFromString<std::string>(parts[1]);
153+
for (size_t i = 2; i < parts.size(); i += 9) {
154+
geometry_msgs::msg::PoseStamped pose_stamped;
155+
pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i]));
156+
pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[i + 1]);
157+
pose_stamped.pose.position.x = BT::convertFromString<double>(parts[i + 2]);
158+
pose_stamped.pose.position.y = BT::convertFromString<double>(parts[i + 3]);
159+
pose_stamped.pose.position.z = BT::convertFromString<double>(parts[i + 4]);
160+
pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[i + 5]);
161+
pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[i + 6]);
162+
pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[i + 7]);
163+
pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[i + 8]);
164+
pose_stamped_array.poses.push_back(pose_stamped);
165+
}
166+
return pose_stamped_array;
167+
}
168+
}
169+
138170
/**
139171
* @brief Parse XML string to nav_msgs::msg::Path
140172
* @param key XML string
@@ -143,7 +175,6 @@ inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const Stri
143175
template<>
144176
inline nav_msgs::msg::Path convertFromString(const StringView key)
145177
{
146-
// 9 real numbers separated by semicolons
147178
auto parts = BT::splitString(key, ';');
148179
if ((parts.size() - 2) % 9 != 0) {
149180
throw std::runtime_error("invalid number of fields for Path attribute)");

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -75,7 +75,7 @@ class ComputePathThroughPosesAction
7575
{
7676
return providedBasicPorts(
7777
{
78-
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
78+
BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
7979
"goals",
8080
"Destinations to plan through"),
8181
BT::InputPort<geometry_msgs::msg::PoseStamped>(

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp

+2-3
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,7 @@
1818
#include <string>
1919
#include <vector>
2020

21-
#include "geometry_msgs/msg/point.hpp"
22-
#include "geometry_msgs/msg/quaternion.hpp"
21+
#include "geometry_msgs/msg/pose_stamped_array.hpp"
2322
#include "nav2_msgs/action/navigate_through_poses.hpp"
2423
#include "nav2_behavior_tree/bt_action_node.hpp"
2524

@@ -74,7 +73,7 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
7473
{
7574
return providedBasicPorts(
7675
{
77-
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
76+
BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
7877
"goals", "Destinations to plan through"),
7978
BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
8079
BT::OutputPort<ActionResult::_error_code_type>(

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

+6-6
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020
#include <string>
2121

2222
#include "rclcpp/rclcpp.hpp"
23-
#include "geometry_msgs/msg/pose_stamped.hpp"
23+
#include "geometry_msgs/msg/pose_stamped_array.hpp"
2424
#include "nav2_behavior_tree/bt_service_node.hpp"
2525
#include "nav2_msgs/srv/get_costs.hpp"
2626

@@ -30,8 +30,6 @@ namespace nav2_behavior_tree
3030
class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
3131
{
3232
public:
33-
typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
34-
3533
/**
3634
* @brief A constructor for nav2_behavior_tree::RemoveInCollisionGoals
3735
* @param service_node_name Service name this node creates a client for
@@ -54,23 +52,25 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
5452
{
5553
return providedBasicPorts(
5654
{
57-
BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
55+
BT::InputPort<geometry_msgs::msg::PoseStampedArray>("input_goals",
56+
"Original goals to remove from"),
5857
BT::InputPort<double>(
5958
"cost_threshold", 254.0,
6059
"Cost threshold for considering a goal in collision"),
6160
BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
6261
BT::InputPort<bool>(
6362
"consider_unknown_as_obstacle", false,
6463
"Whether to consider unknown cost as obstacle"),
65-
BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
64+
BT::OutputPort<geometry_msgs::msg::PoseStampedArray>("output_goals",
65+
"Goals with in-collision goals removed"),
6666
});
6767
}
6868

6969
private:
7070
bool use_footprint_;
7171
bool consider_unknown_as_obstacle_;
7272
double cost_threshold_;
73-
Goals input_goals_;
73+
geometry_msgs::msg::PoseStampedArray input_goals_;
7474
};
7575

7676
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
#include <memory>
2020
#include <string>
2121

22-
#include "geometry_msgs/msg/pose_stamped.hpp"
22+
#include "geometry_msgs/msg/pose_stamped_array.hpp"
2323
#include "nav2_util/geometry_utils.hpp"
2424
#include "nav2_util/robot_utils.hpp"
2525
#include "behaviortree_cpp/action_node.h"
@@ -31,8 +31,6 @@ namespace nav2_behavior_tree
3131
class RemovePassedGoals : public BT::ActionNodeBase
3232
{
3333
public:
34-
typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
35-
3634
RemovePassedGoals(
3735
const std::string & xml_tag_name,
3836
const BT::NodeConfiguration & conf);
@@ -45,8 +43,10 @@ class RemovePassedGoals : public BT::ActionNodeBase
4543
static BT::PortsList providedPorts()
4644
{
4745
return {
48-
BT::InputPort<Goals>("input_goals", "Original goals to remove viapoints from"),
49-
BT::OutputPort<Goals>("output_goals", "Goals with passed viapoints removed"),
46+
BT::InputPort<geometry_msgs::msg::PoseStampedArray>("input_goals",
47+
"Original goals to remove viapoints from"),
48+
BT::OutputPort<geometry_msgs::msg::PoseStampedArray>("output_goals",
49+
"Goals with passed viapoints removed"),
5050
BT::InputPort<double>("radius", 0.5, "radius to goal for it to be considered for removal"),
5151
BT::InputPort<std::string>("global_frame", "Global frame"),
5252
BT::InputPort<std::string>("robot_base_frame", "Robot base frame"),

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@
2121
#include "rclcpp/rclcpp.hpp"
2222

2323
#include "behaviortree_cpp/condition_node.h"
24-
#include "geometry_msgs/msg/pose_stamped.hpp"
24+
#include "geometry_msgs/msg/pose_stamped_array.hpp"
2525
#include "nav2_behavior_tree/bt_utils.hpp"
2626

2727

@@ -59,7 +59,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode
5959
static BT::PortsList providedPorts()
6060
{
6161
return {
62-
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
62+
BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
6363
"goals", "Vector of navigation goals"),
6464
BT::InputPort<geometry_msgs::msg::PoseStamped>(
6565
"goal", "Navigation goal"),
@@ -70,7 +70,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode
7070
bool first_time;
7171
rclcpp::Node::SharedPtr node_;
7272
geometry_msgs::msg::PoseStamped goal_;
73-
std::vector<geometry_msgs::msg::PoseStamped> goals_;
73+
geometry_msgs::msg::PoseStampedArray goals_;
7474
};
7575

7676
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
#include <vector>
2020

2121
#include "behaviortree_cpp/condition_node.h"
22-
#include "geometry_msgs/msg/pose_stamped.hpp"
22+
#include "geometry_msgs/msg/pose_stamped_array.hpp"
2323
#include "nav2_behavior_tree/bt_utils.hpp"
2424

2525
namespace nav2_behavior_tree
@@ -56,7 +56,7 @@ class GoalUpdatedCondition : public BT::ConditionNode
5656
static BT::PortsList providedPorts()
5757
{
5858
return {
59-
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
59+
BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
6060
"goals", "Vector of navigation goals"),
6161
BT::InputPort<geometry_msgs::msg::PoseStamped>(
6262
"goal", "Navigation goal"),
@@ -65,7 +65,7 @@ class GoalUpdatedCondition : public BT::ConditionNode
6565

6666
private:
6767
geometry_msgs::msg::PoseStamped goal_;
68-
std::vector<geometry_msgs::msg::PoseStamped> goals_;
68+
geometry_msgs::msg::PoseStampedArray goals_;
6969
};
7070

7171
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ class GoalUpdatedController : public BT::DecoratorNode
5050
static BT::PortsList providedPorts()
5151
{
5252
return {
53-
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
53+
BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
5454
"goals", "Vector of navigation goals"),
5555
BT::InputPort<geometry_msgs::msg::PoseStamped>(
5656
"goal", "Navigation goal"),
@@ -66,7 +66,7 @@ class GoalUpdatedController : public BT::DecoratorNode
6666

6767
bool goal_was_updated_;
6868
geometry_msgs::msg::PoseStamped goal_;
69-
std::vector<geometry_msgs::msg::PoseStamped> goals_;
69+
geometry_msgs::msg::PoseStampedArray goals_;
7070
};
7171

7272
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ class SpeedController : public BT::DecoratorNode
5858
BT::InputPort<double>("max_rate", 1.0, "Maximum rate"),
5959
BT::InputPort<double>("min_speed", 0.0, "Minimum speed"),
6060
BT::InputPort<double>("max_speed", 0.5, "Maximum speed"),
61-
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
61+
BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
6262
"goals", "Vector of navigation goals"),
6363
BT::InputPort<geometry_msgs::msg::PoseStamped>(
6464
"goal", "Navigation goal"),
@@ -120,7 +120,7 @@ class SpeedController : public BT::DecoratorNode
120120

121121
// current goal
122122
geometry_msgs::msg::PoseStamped goal_;
123-
std::vector<geometry_msgs::msg::PoseStamped> goals_;
123+
geometry_msgs::msg::PoseStampedArray goals_;
124124
};
125125

126126
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -39,15 +39,15 @@ void RemoveInCollisionGoals::on_tick()
3939
getInput("input_goals", input_goals_);
4040
getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
4141

42-
if (input_goals_.empty()) {
42+
if (input_goals_.poses.empty()) {
4343
setOutput("output_goals", input_goals_);
4444
should_send_request_ = false;
4545
return;
4646
}
4747
request_ = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
4848
request_->use_footprint = use_footprint_;
4949

50-
for (const auto & goal : input_goals_) {
50+
for (const auto & goal : input_goals_.poses) {
5151
request_->poses.push_back(goal);
5252
}
5353
}
@@ -63,16 +63,16 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
6363
return BT::NodeStatus::FAILURE;
6464
}
6565

66-
Goals valid_goal_poses;
66+
geometry_msgs::msg::PoseStampedArray valid_goal_poses;
6767
for (size_t i = 0; i < response->costs.size(); ++i) {
6868
if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
6969
response->costs[i] < cost_threshold_)
7070
{
71-
valid_goal_poses.push_back(input_goals_[i]);
71+
valid_goal_poses.poses.push_back(input_goals_.poses[i]);
7272
}
7373
}
7474
// Inform if all goals have been removed
75-
if (valid_goal_poses.empty()) {
75+
if (valid_goal_poses.poses.empty()) {
7676
RCLCPP_INFO(
7777
node_->get_logger(),
7878
"All goals are in collision and have been removed from the list");

‎nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

+6-6
Original file line numberDiff line numberDiff line change
@@ -49,10 +49,10 @@ inline BT::NodeStatus RemovePassedGoals::tick()
4949
initialize();
5050
}
5151

52-
Goals goal_poses;
52+
geometry_msgs::msg::PoseStampedArray goal_poses;
5353
getInput("input_goals", goal_poses);
5454

55-
if (goal_poses.empty()) {
55+
if (goal_poses.poses.empty()) {
5656
setOutput("output_goals", goal_poses);
5757
return BT::NodeStatus::SUCCESS;
5858
}
@@ -61,21 +61,21 @@ inline BT::NodeStatus RemovePassedGoals::tick()
6161

6262
geometry_msgs::msg::PoseStamped current_pose;
6363
if (!nav2_util::getCurrentPose(
64-
current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_,
64+
current_pose, *tf_, goal_poses.poses[0].header.frame_id, robot_base_frame_,
6565
transform_tolerance_))
6666
{
6767
return BT::NodeStatus::FAILURE;
6868
}
6969

7070
double dist_to_goal;
71-
while (goal_poses.size() > 1) {
72-
dist_to_goal = euclidean_distance(goal_poses[0].pose, current_pose.pose);
71+
while (goal_poses.poses.size() > 1) {
72+
dist_to_goal = euclidean_distance(goal_poses.poses[0].pose, current_pose.pose);
7373

7474
if (dist_to_goal > viapoint_achieved_radius_) {
7575
break;
7676
}
7777

78-
goal_poses.erase(goal_poses.begin());
78+
goal_poses.poses.erase(goal_poses.poses.begin());
7979
}
8080

8181
setOutput("output_goals", goal_poses);

‎nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick()
3838
return BT::NodeStatus::SUCCESS;
3939
}
4040

41-
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
41+
geometry_msgs::msg::PoseStampedArray current_goals;
4242
BT::getInputOrBlackboard("goals", current_goals);
4343
geometry_msgs::msg::PoseStamped current_goal;
4444
BT::getInputOrBlackboard("goal", current_goal);

‎nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ BT::NodeStatus GoalUpdatedCondition::tick()
3333
return BT::NodeStatus::FAILURE;
3434
}
3535

36-
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
36+
geometry_msgs::msg::PoseStampedArray current_goals;
3737
geometry_msgs::msg::PoseStamped current_goal;
3838
BT::getInputOrBlackboard("goals", current_goals);
3939
BT::getInputOrBlackboard("goal", current_goal);

‎nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ BT::NodeStatus GoalUpdatedController::tick()
4444

4545
setStatus(BT::NodeStatus::RUNNING);
4646

47-
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
47+
geometry_msgs::msg::PoseStampedArray current_goals;
4848
BT::getInputOrBlackboard("goals", current_goals);
4949
geometry_msgs::msg::PoseStamped current_goal;
5050
BT::getInputOrBlackboard("goal", current_goal);

‎nav2_behavior_tree/plugins/decorator/speed_controller.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ inline BT::NodeStatus SpeedController::tick()
6666
first_tick_ = true;
6767
}
6868

69-
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
69+
geometry_msgs::msg::PoseStampedArray current_goals;
7070
BT::getInputOrBlackboard("goals", current_goals);
7171
geometry_msgs::msg::PoseStamped current_goal;
7272
BT::getInputOrBlackboard("goal", current_goal);

0 commit comments

Comments
 (0)