Skip to content

Commit 3140a6a

Browse files
SteveMacenskialyquantillionanaelle-swasarazinaosmw
authored
Jazzy sync 3: Nov 8, 2024 (#4747)
* Adding non-charging dock support to docking server (for conveyers, pallots, etc) (#4627) * adding non-charging dock support to docking server Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * docs and linting * adding unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Publish optimal trajectory as a Path message (#4640) * Publish optimal trajectory as a Path message Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * move publish_optimal_path to TrajectoryVisualizer + minor refactoring Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * tests added for optimal path publication Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * populate optimal path message in add() Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * move path population in add_marker Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> --------- Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * [collision monitor] Select the observation sources used with each polygon (#4227) * Collision monitor: select specific observation sources for polygon Signed-off-by: asarazin <anaelle.sarazin@robocc.com> * optimization Signed-off-by: asarazin <anaelle.sarazin@robocc.com> * add tests Signed-off-by: asarazin <anaelle.sarazin@robocc.com> --------- Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> * Restore exported BT test utils header files after cmake revamp (#4652) (#4654) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * fix(bt_nodes): Correct default `server_timeout` behavior by using `getInputPortOrBlackboard()` (#4649) Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> * PoseStamped vector specialization (#4607) * PoseStamped vector specialization Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * merge master Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add path Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix size check Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "fix test" This reverts commit 51f54eb. * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * [DWB] Option to limit velocity commands in trajectory generator (#4663) * Option to limit vel cmd through traj generator Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * Cleanup Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * fix linting Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * Update linting Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * uncrustify Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * uncrustify Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> --------- Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * Adding planner server timeout for costmap waiting (#4673) * Adding planner server timeout for costmap waiting Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Adding controller server's costmap timeout as well Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing path longer on approach (#4622) * fixing path longer on approach Signed-off-by: Pradheep <padhupradheep@gmail.com> * removing the short circuit Signed-off-by: Pradheep <padhupradheep@gmail.com> * adding additional layer of check Signed-off-by: Pradheep <padhupradheep@gmail.com> --------- Signed-off-by: Pradheep <padhupradheep@gmail.com> * fix to bt action server logging before bt execution result being ready (#4677) Signed-off-by: DreamWest <sirjamestsao@gmail.com> * Correct paper name for graceful controller Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Added missing action clients in robot_navigator(BasicNavigator) to destroy_node (#4698) * fix: added assisted_teleop_client to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> * fix: added other missing action clients to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> --------- Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> * Fixing SGF in MPPI and Smoother (#4669) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix: handle transition failures in all servers (#4708) * fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * adding support for rest of servers + review comments Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix vel smoother unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing docking server unit testing Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing last bits Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> * [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1 added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart <antoine.gennart@quimesis.be> * Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com> * [RotationShimController] fix: rotate to goal heading (#4724) Add frame_id to goal when rotating towards goal heading, otherwise the transform would fail. This bug was introduced in 30e2cde by not setting the frame_id. Signed-off-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: agennart <antoine.gennart@quimesis.be> * [loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (#4726) * Publish /clock from loopback sim Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * [nav2_costmap_2d] Fix obstacle_layer trying to use RELIABLE QoS Use QoS profile from rclcpp::SensorDataQoS() instead of rmw_qos_profile_t. This solves an issue where the subscriber uses RELIABLE setting even when initialized from rmw_qos_profile_sensor_data. In addition the Subscriber(..., rmw_qos_profile_t) constructor is deprecated in favor of Subscriber(..., rclcpp::QoS) Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * [nav2_smac_planner] fix typos Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Use single quotes Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Fix incorrect doxygen comment (#4741) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> * Updating error logging in Smac collision detector object (#4743) * Updating error logging in Smac configs Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * linting Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * [map_io] Replace std logs by rclcpp logs (#4720) * replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * manual backport to Jazzy of 6b2e244 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * bump to 1.3.3 for jazzy sync Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing backport issue Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing backport of docking linking changes Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> Signed-off-by: Pradheep <padhupradheep@gmail.com> Signed-off-by: DreamWest <sirjamestsao@gmail.com> Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com> Signed-off-by: agennart <antoine.gennart@quimesis.be> Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: alyquantillion <158563995+alyquantillion@users.noreply.github.com> Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: aosmw <116058035+aosmw@users.noreply.github.com> Co-authored-by: Alan <alan.xuefei@googlemail.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Huiyu Leong <26198479+huiyulhy@users.noreply.github.com> Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com> Co-authored-by: DreamWest <sirjamestsao@gmail.com> Co-authored-by: Tiwa Ojo <55967921+tiwaojo@users.noreply.github.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Saitama <gennartan@users.noreply.github.com> Co-authored-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: Daniil Khaninaev <khaninaev@yahoo.com> Co-authored-by: Adi Vardi <57910756+adivardi@users.noreply.github.com> Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
1 parent 3e3506f commit 3140a6a

File tree

132 files changed

+1943
-493
lines changed

Some content is hidden

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

132 files changed

+1943
-493
lines changed

‎nav2_amcl/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>nav2_amcl</name>
5-
<version>1.3.2</version>
5+
<version>1.3.3</version>
66
<description>
77
<p>
88
amcl is a probabilistic localization system for a robot moving in

‎nav2_behavior_tree/CMakeLists.txt

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -246,6 +246,10 @@ install(DIRECTORY test/utils/
246246
DESTINATION include/${PROJECT_NAME}/utils/
247247
)
248248

249+
install(DIRECTORY test/utils/
250+
DESTINATION include/${PROJECT_NAME}/nav2_behavior_tree/test/utils
251+
)
252+
249253
install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
250254
install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME})
251255

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -58,9 +58,7 @@ class BtActionNode : public BT::ActionNodeBase
5858
// Get the required items from the blackboard
5959
auto bt_loop_duration =
6060
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
61-
server_timeout_ =
62-
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
63-
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
61+
getInputOrBlackboard("server_timeout", server_timeout_);
6462
wait_for_service_timeout_ =
6563
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
6664

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -310,18 +310,18 @@ void BtActionServer<ActionT>::executeCallback()
310310

311311
switch (rc) {
312312
case nav2_behavior_tree::BtStatus::SUCCEEDED:
313-
RCLCPP_INFO(logger_, "Goal succeeded");
314313
action_server_->succeeded_current(result);
314+
RCLCPP_INFO(logger_, "Goal succeeded");
315315
break;
316316

317317
case nav2_behavior_tree::BtStatus::FAILED:
318-
RCLCPP_ERROR(logger_, "Goal failed");
319318
action_server_->terminate_current(result);
319+
RCLCPP_ERROR(logger_, "Goal failed");
320320
break;
321321

322322
case nav2_behavior_tree::BtStatus::CANCELED:
323-
RCLCPP_INFO(logger_, "Goal canceled");
324323
action_server_->terminate_all(result);
324+
RCLCPP_INFO(logger_, "Goal canceled");
325325
break;
326326
}
327327

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -56,9 +56,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
5656
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
5757

5858
// Get the required items from the blackboard
59-
server_timeout_ =
60-
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
61-
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
59+
getInputOrBlackboard("server_timeout", server_timeout_);
6260
wait_for_service_timeout_ =
6361
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
6462

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -59,9 +59,7 @@ class BtServiceNode : public BT::ActionNodeBase
5959
// Get the required items from the blackboard
6060
auto bt_loop_duration =
6161
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
62-
server_timeout_ =
63-
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
64-
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
62+
getInputOrBlackboard("server_timeout", server_timeout_);
6563
wait_for_service_timeout_ =
6664
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
6765

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,13 +17,15 @@
1717

1818
#include <string>
1919
#include <set>
20+
#include <vector>
2021

2122
#include "rclcpp/time.hpp"
2223
#include "rclcpp/node.hpp"
2324
#include "behaviortree_cpp/behavior_tree.h"
2425
#include "geometry_msgs/msg/point.hpp"
2526
#include "geometry_msgs/msg/quaternion.hpp"
2627
#include "geometry_msgs/msg/pose_stamped.hpp"
28+
#include "nav_msgs/msg/path.hpp"
2729

2830
namespace BT
2931
{
@@ -102,6 +104,70 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
102104
}
103105
}
104106

107+
/**
108+
* @brief Parse XML string to std::vector<geometry_msgs::msg::PoseStamped>
109+
* @param key XML string
110+
* @return std::vector<geometry_msgs::msg::PoseStamped>
111+
*/
112+
template<>
113+
inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const StringView key)
114+
{
115+
// 9 real numbers separated by semicolons
116+
auto parts = BT::splitString(key, ';');
117+
if (parts.size() % 9 != 0) {
118+
throw std::runtime_error("invalid number of fields for std::vector<PoseStamped> attribute)");
119+
} else {
120+
std::vector<geometry_msgs::msg::PoseStamped> poses;
121+
for (size_t i = 0; i < parts.size(); i += 9) {
122+
geometry_msgs::msg::PoseStamped pose_stamped;
123+
pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i]));
124+
pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[i + 1]);
125+
pose_stamped.pose.position.x = BT::convertFromString<double>(parts[i + 2]);
126+
pose_stamped.pose.position.y = BT::convertFromString<double>(parts[i + 3]);
127+
pose_stamped.pose.position.z = BT::convertFromString<double>(parts[i + 4]);
128+
pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[i + 5]);
129+
pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[i + 6]);
130+
pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[i + 7]);
131+
pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[i + 8]);
132+
poses.push_back(pose_stamped);
133+
}
134+
return poses;
135+
}
136+
}
137+
138+
/**
139+
* @brief Parse XML string to nav_msgs::msg::Path
140+
* @param key XML string
141+
* @return nav_msgs::msg::Path
142+
*/
143+
template<>
144+
inline nav_msgs::msg::Path convertFromString(const StringView key)
145+
{
146+
// 9 real numbers separated by semicolons
147+
auto parts = BT::splitString(key, ';');
148+
if ((parts.size() - 2) % 9 != 0) {
149+
throw std::runtime_error("invalid number of fields for Path attribute)");
150+
} else {
151+
nav_msgs::msg::Path path;
152+
path.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
153+
path.header.frame_id = BT::convertFromString<std::string>(parts[1]);
154+
for (size_t i = 2; i < parts.size(); i += 9) {
155+
geometry_msgs::msg::PoseStamped pose_stamped;
156+
path.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i]));
157+
pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[i + 1]);
158+
pose_stamped.pose.position.x = BT::convertFromString<double>(parts[i + 2]);
159+
pose_stamped.pose.position.y = BT::convertFromString<double>(parts[i + 3]);
160+
pose_stamped.pose.position.z = BT::convertFromString<double>(parts[i + 4]);
161+
pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[i + 5]);
162+
pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[i + 6]);
163+
pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[i + 7]);
164+
pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[i + 8]);
165+
path.poses.push_back(pose_stamped);
166+
}
167+
return path;
168+
}
169+
}
170+
105171
/**
106172
* @brief Parse XML string to std::chrono::milliseconds
107173
* @param key XML string

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include "nav2_util/geometry_utils.hpp"
2424
#include "nav2_util/robot_utils.hpp"
2525
#include "behaviortree_cpp/action_node.h"
26+
#include "nav2_behavior_tree/bt_utils.hpp"
2627

2728
namespace nav2_behavior_tree
2829
{

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include "rclcpp/rclcpp.hpp"
2525
#include "geometry_msgs/msg/pose_stamped.hpp"
2626
#include "tf2_ros/buffer.h"
27+
#include "nav2_behavior_tree/bt_utils.hpp"
2728

2829
namespace nav2_behavior_tree
2930
{

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222

2323
#include "behaviortree_cpp/condition_node.h"
2424
#include "geometry_msgs/msg/pose_stamped.hpp"
25+
#include "nav2_behavior_tree/bt_utils.hpp"
2526

2627

2728
namespace nav2_behavior_tree

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include "rclcpp/rclcpp.hpp"
2222
#include "behaviortree_cpp/condition_node.h"
2323
#include "tf2_ros/buffer.h"
24+
#include "nav2_behavior_tree/bt_utils.hpp"
2425

2526
namespace nav2_behavior_tree
2627
{

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020

2121
#include "behaviortree_cpp/condition_node.h"
2222
#include "geometry_msgs/msg/pose_stamped.hpp"
23+
#include "nav2_behavior_tree/bt_utils.hpp"
2324

2425
namespace nav2_behavior_tree
2526
{

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include <string>
1919
#include "behaviortree_cpp/behavior_tree.h"
20+
#include "nav2_behavior_tree/bt_utils.hpp"
2021

2122
namespace nav2_behavior_tree
2223
{

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include "rclcpp/rclcpp.hpp"
2121
#include "behaviortree_cpp/condition_node.h"
2222
#include "nav_msgs/msg/path.hpp"
23+
#include "nav2_behavior_tree/bt_utils.hpp"
2324

2425
namespace nav2_behavior_tree
2526
{

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include "tf2_ros/buffer.h"
2424

2525
#include "behaviortree_cpp/decorator_node.h"
26+
#include "nav2_behavior_tree/bt_utils.hpp"
2627

2728
namespace nav2_behavior_tree
2829
{

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323

2424
#include "rclcpp/rclcpp.hpp"
2525
#include "geometry_msgs/msg/pose_stamped.hpp"
26+
#include "nav2_behavior_tree/bt_utils.hpp"
2627

2728
namespace nav2_behavior_tree
2829
{

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include "nav2_util/odometry_utils.hpp"
2626

2727
#include "behaviortree_cpp/decorator_node.h"
28+
#include "nav2_behavior_tree/bt_utils.hpp"
2829

2930
namespace nav2_behavior_tree
3031
{

‎nav2_behavior_tree/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>nav2_behavior_tree</name>
5-
<version>1.3.2</version>
5+
<version>1.3.3</version>
66
<description>Nav2 behavior tree wrappers, nodes, and utilities</description>
77
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
88
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

‎nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,6 @@
2020
#include "nav2_util/geometry_utils.hpp"
2121

2222
#include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
23-
#include "nav2_behavior_tree/bt_utils.hpp"
2423

2524
namespace nav2_behavior_tree
2625
{

‎nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,6 @@
2020
#include "nav2_util/geometry_utils.hpp"
2121

2222
#include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
23-
#include "nav2_behavior_tree/bt_utils.hpp"
2423

2524
namespace nav2_behavior_tree
2625
{

‎nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,6 @@
1616
#include <string>
1717

1818
#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
19-
#include "nav2_behavior_tree/bt_utils.hpp"
2019

2120
namespace nav2_behavior_tree
2221
{

‎nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,6 @@
2020
#include "nav2_util/node_utils.hpp"
2121

2222
#include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
23-
#include "nav2_behavior_tree/bt_utils.hpp"
2423

2524
namespace nav2_behavior_tree
2625
{

‎nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,6 @@
1515
#include <string>
1616
#include <vector>
1717
#include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
18-
#include "nav2_behavior_tree/bt_utils.hpp"
1918

2019
namespace nav2_behavior_tree
2120
{

‎nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,6 @@
1313
// limitations under the License.
1414

1515
#include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
16-
#include "nav2_behavior_tree/bt_utils.hpp"
1716

1817
namespace nav2_behavior_tree
1918
{

‎nav2_behavior_tree/plugins/decorator/distance_controller.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,6 @@
2626
#include "behaviortree_cpp/decorator_node.h"
2727

2828
#include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
29-
#include "nav2_behavior_tree/bt_utils.hpp"
3029

3130
namespace nav2_behavior_tree
3231
{

‎nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,6 @@
1919
#include "geometry_msgs/msg/pose_stamped.hpp"
2020
#include "behaviortree_cpp/decorator_node.h"
2121
#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
22-
#include "nav2_behavior_tree/bt_utils.hpp"
2322

2423
namespace nav2_behavior_tree
2524
{

‎nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,9 +34,10 @@ bool PathLongerOnApproach::isPathUpdated(
3434
nav_msgs::msg::Path & new_path,
3535
nav_msgs::msg::Path & old_path)
3636
{
37-
return new_path != old_path && old_path.poses.size() != 0 &&
37+
return old_path.poses.size() != 0 &&
3838
new_path.poses.size() != 0 &&
39-
old_path.poses.back().pose == new_path.poses.back().pose;
39+
new_path.poses.size() != old_path.poses.size() &&
40+
old_path.poses.back().pose.position == new_path.poses.back().pose.position;
4041
}
4142

4243
bool PathLongerOnApproach::isRobotInGoalProximity(
@@ -64,7 +65,7 @@ inline BT::NodeStatus PathLongerOnApproach::tick()
6465

6566
if (first_time_ == false) {
6667
if (old_path_.poses.empty() || new_path_.poses.empty() ||
67-
old_path_.poses.back() != new_path_.poses.back())
68+
old_path_.poses.back().pose != new_path_.poses.back().pose)
6869
{
6970
first_time_ = true;
7071
}

‎nav2_behavior_tree/plugins/decorator/speed_controller.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,6 @@
1919
#include "nav2_util/geometry_utils.hpp"
2020

2121
#include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
22-
#include "nav2_behavior_tree/bt_utils.hpp"
2322

2423
namespace nav2_behavior_tree
2524
{

‎nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,6 @@
2323

2424
#include "utils/test_behavior_tree_fixture.hpp"
2525
#include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
26-
#include "nav2_behavior_tree/bt_utils.hpp"
2726

2827
using namespace std::chrono; // NOLINT
2928
using namespace std::chrono_literals; // NOLINT

‎nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -36,8 +36,6 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree
3636
poses1.push_back(goal1);
3737
config_->blackboard->set("goal", goal1);
3838
config_->blackboard->set("goals", poses1);
39-
config_->input_ports["goals"] = "";
40-
config_->input_ports["goal"] = "";
4139
bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedController>(
4240
"goal_updated_controller", *config_);
4341
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();

0 commit comments

Comments
 (0)