Skip to content

Commit 8a7ca39

Browse files
Nils-ChristianIsekeNils-ChristianIseke
committed
Merge remote-tracking branch 'origin/main' into precommit
Signed-off-by: Nils-ChristianIseke <[email protected]>
1 parent 39adf59 commit 8a7ca39

File tree

52 files changed

+503
-291
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

52 files changed

+503
-291
lines changed

‎nav2_amcl/include/nav2_amcl/amcl_node.hpp‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,15 @@
2828
#include <utility>
2929
#include <vector>
3030

31-
#include "geometry_msgs/msg/pose_stamped.hpp"
31+
// For compatibility with Jazzy
32+
#include "rclcpp/version.h"
33+
#if RCLCPP_VERSION_GTE(29, 0, 0)
3234
#include "message_filters/subscriber.hpp"
35+
#else
36+
#include "message_filters/subscriber.h"
37+
#endif
38+
39+
#include "geometry_msgs/msg/pose_stamped.hpp"
3340
#include "nav2_util/lifecycle_node.hpp"
3441
#include "nav2_amcl/motion_model/motion_model.hpp"
3542
#include "nav2_amcl/sensors/laser/laser.hpp"

‎nav2_amcl/src/amcl_node.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,6 @@
2828
#include <utility>
2929
#include <vector>
3030

31-
#include "message_filters/subscriber.hpp"
3231
#include "nav2_amcl/angleutils.hpp"
3332
#include "nav2_util/geometry_utils.hpp"
3433
#include "nav2_amcl/pf/pf.hpp"

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -24,9 +24,9 @@
2424
#include "behaviortree_cpp/behavior_tree.h"
2525
#include "geometry_msgs/msg/point.hpp"
2626
#include "geometry_msgs/msg/pose_stamped.hpp"
27-
#include "geometry_msgs/msg/pose_stamped_array.hpp"
2827
#include "geometry_msgs/msg/quaternion.hpp"
2928
#include "nav_msgs/msg/path.hpp"
29+
#include "nav_msgs/msg/goals.hpp"
3030

3131
namespace BT
3232
{
@@ -136,20 +136,20 @@ inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const Stri
136136
}
137137

138138
/**
139-
* @brief Parse XML string to geometry_msgs::msg::PoseStampedArray
139+
* @brief Parse XML string to nav_msgs::msg::Goals
140140
* @param key XML string
141-
* @return geometry_msgs::msg::PoseStampedArray
141+
* @return nav_msgs::msg::Goals
142142
*/
143143
template<>
144-
inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView key)
144+
inline nav_msgs::msg::Goals convertFromString(const StringView key)
145145
{
146146
auto parts = BT::splitString(key, ';');
147147
if ((parts.size() - 2) % 9 != 0) {
148-
throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)");
148+
throw std::runtime_error("invalid number of fields for Goals attribute)");
149149
} 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]);
150+
nav_msgs::msg::Goals goals_array;
151+
goals_array.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
152+
goals_array.header.frame_id = BT::convertFromString<std::string>(parts[1]);
153153
for (size_t i = 2; i < parts.size(); i += 9) {
154154
geometry_msgs::msg::PoseStamped pose_stamped;
155155
pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i]));
@@ -161,9 +161,9 @@ inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView k
161161
pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[i + 6]);
162162
pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[i + 7]);
163163
pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[i + 8]);
164-
pose_stamped_array.poses.push_back(pose_stamped);
164+
goals_array.goals.push_back(pose_stamped);
165165
}
166-
return pose_stamped_array;
166+
return goals_array;
167167
}
168168
}
169169

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -80,7 +80,7 @@ class ComputePathThroughPosesAction
8080
{
8181
return providedBasicPorts(
8282
{
83-
BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
83+
BT::InputPort<nav_msgs::msg::Goals>(
8484
"goals",
8585
"Destinations to plan through"),
8686
BT::InputPort<geometry_msgs::msg::PoseStamped>(

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
#include <string>
1919
#include <vector>
2020

21-
#include "geometry_msgs/msg/pose_stamped_array.hpp"
21+
#include "nav_msgs/msg/goals.hpp"
2222
#include "nav2_msgs/action/navigate_through_poses.hpp"
2323
#include "nav2_behavior_tree/bt_action_node.hpp"
2424

@@ -73,7 +73,7 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
7373
{
7474
return providedBasicPorts(
7575
{
76-
BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
76+
BT::InputPort<nav_msgs::msg::Goals>(
7777
"goals", "Destinations to plan through"),
7878
BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
7979
BT::OutputPort<ActionResult::_error_code_type>(

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

Lines changed: 4 additions & 4 deletions
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_array.hpp"
23+
#include "nav_msgs/msg/goals.hpp"
2424
#include "nav2_behavior_tree/bt_service_node.hpp"
2525
#include "nav2_msgs/srv/get_costs.hpp"
2626

@@ -52,7 +52,7 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
5252
{
5353
return providedBasicPorts(
5454
{
55-
BT::InputPort<geometry_msgs::msg::PoseStampedArray>("input_goals",
55+
BT::InputPort<nav_msgs::msg::Goals>("input_goals",
5656
"Original goals to remove from"),
5757
BT::InputPort<double>(
5858
"cost_threshold", 254.0,
@@ -61,7 +61,7 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
6161
BT::InputPort<bool>(
6262
"consider_unknown_as_obstacle", false,
6363
"Whether to consider unknown cost as obstacle"),
64-
BT::OutputPort<geometry_msgs::msg::PoseStampedArray>("output_goals",
64+
BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
6565
"Goals with in-collision goals removed"),
6666
});
6767
}
@@ -70,7 +70,7 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
7070
bool use_footprint_;
7171
bool consider_unknown_as_obstacle_;
7272
double cost_threshold_;
73-
geometry_msgs::msg::PoseStampedArray input_goals_;
73+
nav_msgs::msg::Goals input_goals_;
7474
};
7575

7676
} // namespace nav2_behavior_tree

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

Lines changed: 3 additions & 3 deletions
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_array.hpp"
22+
#include "nav_msgs/msg/goals.hpp"
2323
#include "nav2_util/geometry_utils.hpp"
2424
#include "nav2_util/robot_utils.hpp"
2525
#include "behaviortree_cpp/action_node.h"
@@ -43,9 +43,9 @@ class RemovePassedGoals : public BT::ActionNodeBase
4343
static BT::PortsList providedPorts()
4444
{
4545
return {
46-
BT::InputPort<geometry_msgs::msg::PoseStampedArray>("input_goals",
46+
BT::InputPort<nav_msgs::msg::Goals>("input_goals",
4747
"Original goals to remove viapoints from"),
48-
BT::OutputPort<geometry_msgs::msg::PoseStampedArray>("output_goals",
48+
BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
4949
"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>("robot_base_frame", "Robot base frame"),

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

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,12 @@ class AreErrorCodesPresent : public BT::ConditionNode
3434
const BT::NodeConfiguration & conf)
3535
: BT::ConditionNode(condition_name, conf)
3636
{
37-
getInput<std::set<uint16_t>>("error_codes_to_check", error_codes_to_check_); //NOLINT
37+
std::vector<int> error_codes_to_check_vector;
38+
getInput("error_codes_to_check", error_codes_to_check_vector); //NOLINT
39+
40+
error_codes_to_check_ = std::set<uint16_t>(
41+
error_codes_to_check_vector.begin(),
42+
error_codes_to_check_vector.end());
3843
}
3944

4045
AreErrorCodesPresent() = delete;
@@ -55,7 +60,7 @@ class AreErrorCodesPresent : public BT::ConditionNode
5560
return
5661
{
5762
BT::InputPort<uint16_t>("error_code", "The active error codes"), //NOLINT
58-
BT::InputPort<std::set<uint16_t>>("error_codes_to_check", "Error codes to check")//NOLINT
63+
BT::InputPort<std::vector<int>>("error_codes_to_check", "Error codes to check")//NOLINT
5964
};
6065
}
6166

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

Lines changed: 3 additions & 3 deletions
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_array.hpp"
24+
#include "nav_msgs/msg/goals.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<geometry_msgs::msg::PoseStampedArray>(
62+
BT::InputPort<nav_msgs::msg::Goals>(
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-
geometry_msgs::msg::PoseStampedArray goals_;
73+
nav_msgs::msg::Goals goals_;
7474
};
7575

7676
} // namespace nav2_behavior_tree

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

Lines changed: 3 additions & 3 deletions
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_array.hpp"
22+
#include "nav_msgs/msg/goals.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<geometry_msgs::msg::PoseStampedArray>(
59+
BT::InputPort<nav_msgs::msg::Goals>(
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-
geometry_msgs::msg::PoseStampedArray goals_;
68+
nav_msgs::msg::Goals goals_;
6969
};
7070

7171
} // namespace nav2_behavior_tree

0 commit comments

Comments
 (0)