Skip to content

Commit 0aee198

Browse files
SteveMacenskiDylanDeCoeyer-Quimesisajtudelamikefergusontonynajjar
authored
Jazzy sync 5: Feb 4, 2025 (#4902)
* nav2_smac_planner: handle corner case where start and goal are on the same cell (#4793) * nav2_smac_planner: handle corner case where start and goal are on the same cell This case was already properly handled in the smac_planner_2d, but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case. This commit fixes issue #4792. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: use goal orientation when path is made of one point Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: publish raw path also when start and goal are on the same cell Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * nav2_smac_planner: add corner case to unit tests Add a plan where the start and goal are placed on the same cell. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> --------- Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * creating auto-transition option for nav2_util::LifecycleNode (#4804) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix trajectory generation bug in docking controller (#4807) * Fix trajectory in docking controller Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Ceil and remove resolution Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update nav2_docking/opennav_docking/src/controller.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update nav2_docking/opennav_docking/src/controller.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * graceful_controller: implement iterative selection of control points (#4795) * initial pass at iterative Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * add v_angular_min_in_place Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * add orientation filter, fix remaining TODOs Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * try to increase coverage, fixup minor test issues Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * address review comments Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * review comments * update defaults * rename to in_place_collision_resolution Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * revert change in default Signed-off-by: Michael Ferguson <mfergs7@gmail.com> --------- Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * fix bug in use of v_angular_min_in_place (#4813) Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * publish motion target as pose (#4839) Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * nav2_behaviors: drive_on_heading: return TIMEOUT error code when exceeding time allowance (#4836) Until now, the NONE error code was returned when exceeding the time allowance. Let's return the more appropriate TIMEOUT error code instead. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * fix bug in orientation filtering (#4840) * fix bug in orientation filtering some global planners output all zeros for orientation, however the plan is in the global frame. when transforming to the local frame, the orientation is no longer zero. Instead of comparing to zero, we simply check if all the orientations in the middle of the plan are equal Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * account for floating point error Signed-off-by: Michael Ferguson <mfergs7@gmail.com> --------- Signed-off-by: Michael Ferguson <mfergs7@gmail.com> * Adapt GoalUpdater to update goals as well (#4771) * Add IsStoppedBTNode Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add topic name + reformat Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix comment Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix abs Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove log Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add getter functions for raw twist Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove unused code Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use odomsmoother Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * update groot Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * reset at success Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * FIX velocity_threshold_ Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix stopped Node Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add tests to odometry_utils Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix linting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Adapt goalUpdater to modify goals as well Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * change name of msg Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Make input goals be Goals again for compatibility Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "fix" This reverts commit 8303cdc. Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * refactoring Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * ament Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * ignore if no timestamps Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * facepalm Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * update groot nodes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Use PoseStampedArray Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use geometry_msgs Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use geometry_msgs Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * more fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * . Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * revert Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * . Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add common_interfaces Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use PoseStampedArray Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * reformat Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * revert Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * revert Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix warn msg Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * improve Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix format Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * change to info Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix simple smoother failing during final approach (#4817) * new test case for end of path approach Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> * modify tests to match the more permissive smoother policy Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> * implement steve's suggestions Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> --------- Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> * Add acc limit consideration to avoid overshooting in RotationShimController (#4864) * Add acc limit consideration to avoid overshoot in RotationShimController Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add acceleration limit tests to RotationShimController Signed-off-by: RBT22 <rozgonyibalint@gmail.com> --------- Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Fix flaky ness of opennav_docking test_docking_server (#4878) (#4879) Call publish() (odom -> base_link tf) at startup to kick things off and spin 10 times(1 second) TF, so that it has a chance to propogate to the docking_server so that it will accept an action request. Previously it was only spinning once, hoping the timer would fire and call publish fast enough for it to propogate to the docking_server so that it is able to accept the first 'dock_robot' action request Signed-off-by: Mike Wake <macwake@gmail.com> * [BtActionNode] [BtServiceNode] clear between calls (#4887) Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * dwb_critics flaky test - lineCost coordinates must be within costmap (#4889) (#4884) There is no protection/checks in the pathway from lineCost to costmap_2d::getIndex(mx, my) for grid coordinates that exceed the of bounds of the allocated costmap. (presumably for speed) This test was triggering an off by one error attempting to read the the 2500 byte costmap at byte 2503 costmap size 50x50. getIndex(3, 50) = my * size_x_ + mx; = 50 * 50 + 3; = 2503 Signed-off-by: Mike Wake <macwake@gmail.com> * Add option to use open-loop control with Rotation Shim (#4880) * Initial implementation Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * replace feedback param with closed_loop Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Reset last_angular_vel_ in activate method Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add closed_loop parameter to dynamicParametersCallback Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Add tests Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Override reset function Signed-off-by: RBT22 <rozgonyibalint@gmail.com> --------- Signed-off-by: RBT22 <rozgonyibalint@gmail.com> * Fix unstable test in nav2 util (#4894) * Fix unstable test in nav2 util Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Change 5s to 1s Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update bt2img syntax and bt pics (#4900) Signed-off-by: Maurice-1235 <mauricepurnawan@gmail.com> * bumping to 1.3.5 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Revert "Adapt GoalUpdater to update goals as well (#4771)" This reverts commit 55d7387. --------- Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: Michael Ferguson <mfergs7@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com> Signed-off-by: RBT22 <rozgonyibalint@gmail.com> Signed-off-by: Mike Wake <macwake@gmail.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Maurice-1235 <mauricepurnawan@gmail.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: Alberto Tudela <ajtudela@gmail.com> Co-authored-by: Michael Ferguson <mfergs7@gmail.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Ray Ferric <63957587+rayferric@users.noreply.github.com> Co-authored-by: Balint Rozgonyi <43723477+RBT22@users.noreply.github.com> Co-authored-by: ewak <ewak@users.noreply.github.com> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: mini-1235 <58433591+mini-1235@users.noreply.github.com>
1 parent 44896e9 commit 0aee198

File tree

75 files changed

+817
-389
lines changed

Some content is hidden

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

75 files changed

+817
-389
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.4</version>
5+
<version>1.3.5</version>
66
<description>
77
<p>
88
amcl is a probabilistic localization system for a robot moving in

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -194,6 +194,10 @@ class BtActionNode : public BT::ActionNodeBase
194194
// reset the flag to send the goal or not, allowing the user the option to set it in on_tick
195195
should_send_goal_ = true;
196196

197+
// Clear the input and output messages to make sure we have no leftover from previous calls
198+
goal_ = typename ActionT::Goal();
199+
result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
200+
197201
// user defined callback, may modify "should_send_goal_".
198202
on_tick();
199203

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,9 @@ class BtServiceNode : public BT::ActionNodeBase
138138
// allowing the user the option to set it in on_tick
139139
should_send_request_ = true;
140140

141+
// Clear the input request to make sure we have no leftover from previous calls
142+
request_ = std::make_shared<typename ServiceT::Request>();
143+
141144
// user defined callback, may modify "should_send_request_".
142145
on_tick();
143146

‎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.4</version>
5+
<version>1.3.5</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_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
102102
RCLCPP_WARN(
103103
this->logger_,
104104
"Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading");
105-
return ResultStatus{Status::FAILED, ActionT::Result::NONE};
105+
return ResultStatus{Status::FAILED, ActionT::Result::TIMEOUT};
106106
}
107107

108108
geometry_msgs::msg::PoseStamped current_pose;

‎nav2_behaviors/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_behaviors</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>Nav2 behavior server</description>
77
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
88
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

‎nav2_bringup/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_bringup</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>Bringup scripts and configurations for the Nav2 stack</description>
77
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
88
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

‎nav2_bt_navigator/doc/parallel_w_recovery.png

70.7 KB
Loading

‎nav2_bt_navigator/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_bt_navigator</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>Nav2 BT Navigator Server</description>
77
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
88
<license>Apache-2.0</license>

‎nav2_collision_monitor/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_collision_monitor</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>Collision Monitor</description>
77
<maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
88
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

‎nav2_common/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_common</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>Common support functionality used throughout the navigation 2 stack</description>
77
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
88
<license>Apache-2.0</license>

‎nav2_constrained_smoother/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_constrained_smoother</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>Ceres constrained smoother</description>
77
<maintainer email="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
88
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

‎nav2_controller/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_controller</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>Controller action interface</description>
77
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
88
<license>Apache-2.0</license>

‎nav2_core/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_core</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>A set of headers for plugins core to the Nav2 stack</description>
77
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
88
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>

‎nav2_costmap_2d/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_costmap_2d</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>
77
This package provides an implementation of a 2D costmap that takes in sensor
88
data from the world, builds a 2D or 3D occupancy grid of the data (depending

‎nav2_docking/opennav_docking/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>opennav_docking</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>A Task Server for robot charger docking</description>
77
<maintainer email="steve@opennav.org">Steve Macenski</maintainer>
88
<license>Apache-2.0</license>

‎nav2_docking/opennav_docking/src/controller.cpp

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -144,7 +144,10 @@ bool Controller::isTrajectoryCollisionFree(
144144
}
145145

146146
// Generate path
147-
for (double t = 0; t < projection_time_; t += simulation_time_step_) {
147+
double distance = std::numeric_limits<double>::max();
148+
unsigned int max_iter = static_cast<unsigned int>(ceil(projection_time_ / simulation_time_step_));
149+
150+
do{
148151
// Apply velocities to calculate next pose
149152
next_pose.pose = control_law_->calculateNextPose(
150153
simulation_time_step_, target_pose, next_pose.pose, backward);
@@ -177,7 +180,10 @@ bool Controller::isTrajectoryCollisionFree(
177180
trajectory_pub_->publish(trajectory);
178181
return false;
179182
}
180-
}
183+
184+
// Check if we reach the goal
185+
distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
186+
}while(distance > 1e-2 && trajectory.poses.size() < max_iter);
181187

182188
trajectory_pub_->publish(trajectory);
183189

‎nav2_docking/opennav_docking/test/test_docking_server.py

Lines changed: 58 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -19,23 +19,37 @@
1919
from action_msgs.msg import GoalStatus
2020
from geometry_msgs.msg import TransformStamped, Twist
2121
from launch import LaunchDescription
22+
# from launch.actions import SetEnvironmentVariable
2223
from launch_ros.actions import Node
2324
import launch_testing
25+
import launch_testing.actions
26+
import launch_testing.asserts
27+
import launch_testing.markers
28+
import launch_testing.util
2429
from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot
2530
import pytest
2631
import rclpy
27-
from rclpy.action import ActionClient, ActionServer
32+
from rclpy.action.client import ActionClient
33+
from rclpy.action.server import ActionServer
2834
from sensor_msgs.msg import BatteryState
2935
from tf2_ros import TransformBroadcaster
3036

3137

3238
# This test can be run standalone with:
3339
# python3 -u -m pytest test_docking_server.py -s
3440

41+
# If python3-flaky is installed, you can run the test multiple times to
42+
# try to identify flaky ness.
43+
# python3 -u -m pytest --force-flaky --min-passes 3 --max-runs 5 -s -v test_docking_server.py
44+
3545
@pytest.mark.rostest
46+
# @pytest.mark.flaky
47+
# @pytest.mark.flaky(max_runs=5, min_passes=3)
3648
def generate_test_description():
3749

3850
return LaunchDescription([
51+
# SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
52+
# SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
3953
Node(
4054
package='opennav_docking',
4155
executable='opennav_docking',
@@ -74,21 +88,26 @@ class TestDockingServer(unittest.TestCase):
7488
@classmethod
7589
def setUpClass(cls):
7690
rclpy.init()
77-
# Latest odom -> base_link
78-
cls.x = 0.0
79-
cls.y = 0.0
80-
cls.theta = 0.0
81-
# Track charge state
82-
cls.is_charging = False
83-
# Latest command velocity
84-
cls.command = Twist()
85-
cls.node = rclpy.create_node('test_docking_server')
8691

8792
@classmethod
8893
def tearDownClass(cls):
89-
cls.node.destroy_node()
9094
rclpy.shutdown()
9195

96+
def setUp(self):
97+
# Create a ROS node for tests
98+
# Latest odom -> base_link
99+
self.x = 0.0
100+
self.y = 0.0
101+
self.theta = 0.0
102+
# Track charge state
103+
self.is_charging = False
104+
# Latest command velocity
105+
self.command = Twist()
106+
self.node = rclpy.create_node('test_docking_server')
107+
108+
def tearDown(self):
109+
self.node.destroy_node()
110+
92111
def command_velocity_callback(self, msg):
93112
self.node.get_logger().info('Command: %f %f' % (msg.linear.x, msg.angular.z))
94113
self.command = msg
@@ -175,24 +194,32 @@ def test_docking_server(self):
175194
'navigate_to_pose',
176195
self.nav_execute_callback)
177196

178-
# Spin once so that TF is published
179-
rclpy.spin_once(self.node, timeout_sec=0.1)
197+
# Publish transform
198+
self.publish()
199+
200+
# Run for 1 seconds to allow tf to propogate
201+
for _ in range(10):
202+
rclpy.spin_once(self.node, timeout_sec=0.1)
203+
time.sleep(0.1)
180204

181205
# Test docking action
182206
self.action_result = []
183-
self.dock_action_client.wait_for_server(timeout_sec=5.0)
207+
assert self.dock_action_client.wait_for_server(timeout_sec=5.0), \
208+
'dock_robot service not available'
209+
184210
goal = DockRobot.Goal()
185211
goal.use_dock_id = True
186212
goal.dock_id = 'test_dock'
187213
future = self.dock_action_client.send_goal_async(
188214
goal, feedback_callback=self.action_feedback_callback)
189215
rclpy.spin_until_future_complete(self.node, future)
190216
self.goal_handle = future.result()
191-
assert self.goal_handle.accepted
217+
assert self.goal_handle is not None, 'goal_handle should not be None'
218+
assert self.goal_handle.accepted, 'goal_handle not accepted'
192219
result_future_original = self.goal_handle.get_result_async()
193220

194221
# Run for 2 seconds
195-
for i in range(20):
222+
for _ in range(20):
196223
rclpy.spin_once(self.node, timeout_sec=0.1)
197224
time.sleep(0.1)
198225

@@ -201,7 +228,8 @@ def test_docking_server(self):
201228
goal, feedback_callback=self.action_feedback_callback)
202229
rclpy.spin_until_future_complete(self.node, future)
203230
self.goal_handle = future.result()
204-
assert self.goal_handle.accepted
231+
assert self.goal_handle is not None, 'goal_handle should not be None'
232+
assert self.goal_handle.accepted, 'goal_handle not accepted'
205233
result_future = self.goal_handle.get_result_async()
206234
rclpy.spin_until_future_complete(self.node, result_future)
207235
self.action_result.append(result_future.result())
@@ -216,7 +244,7 @@ def test_docking_server(self):
216244
self.node.get_logger().info('Goal preempted')
217245

218246
# Run for 0.5 seconds
219-
for i in range(5):
247+
for _ in range(5):
220248
rclpy.spin_once(self.node, timeout_sec=0.1)
221249
time.sleep(0.1)
222250

@@ -230,7 +258,8 @@ def test_docking_server(self):
230258
goal, feedback_callback=self.action_feedback_callback)
231259
rclpy.spin_until_future_complete(self.node, future)
232260
self.goal_handle = future.result()
233-
assert self.goal_handle.accepted
261+
assert self.goal_handle is not None, 'goal_handle should not be None'
262+
assert self.goal_handle.accepted, 'goal_handle not accepted'
234263
result_future = self.goal_handle.get_result_async()
235264
rclpy.spin_until_future_complete(self.node, result_future)
236265
self.action_result.append(result_future.result())
@@ -247,10 +276,19 @@ def test_docking_server(self):
247276
future = self.undock_action_client.send_goal_async(goal)
248277
rclpy.spin_until_future_complete(self.node, future)
249278
self.goal_handle = future.result()
250-
assert self.goal_handle.accepted
279+
assert self.goal_handle is not None, 'goal_handle should not be None'
280+
assert self.goal_handle.accepted, 'goal_handle not accepted'
251281
result_future = self.goal_handle.get_result_async()
252282
rclpy.spin_until_future_complete(self.node, result_future)
253283
self.action_result.append(result_future.result())
254284

255285
self.assertEqual(self.action_result[3].status, GoalStatus.STATUS_SUCCEEDED)
256286
self.assertTrue(self.action_result[3].result.success)
287+
288+
289+
@launch_testing.post_shutdown_test()
290+
class TestProcessOutput(unittest.TestCase):
291+
292+
def test_exit_code(self, proc_info):
293+
# Check that all processes in the launch exit with code 0
294+
launch_testing.asserts.assertExitCodes(proc_info)

‎nav2_docking/opennav_docking_bt/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>opennav_docking_bt</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>A set of BT nodes and XMLs for docking</description>
77
<maintainer email="steve@opennav.org">Steve Macenski</maintainer>
88
<license>Apache-2.0</license>

‎nav2_docking/opennav_docking_core/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>opennav_docking_core</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>A set of headers for plugins core to the opennav docking framework</description>
77
<maintainer email="steve@opennav.org">Steve Macenski</maintainer>
88
<license>Apache-2.0</license>

‎nav2_dwb_controller/costmap_queue/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="2">
33
<name>costmap_queue</name>
4-
<version>1.3.4</version>
4+
<version>1.3.5</version>
55
<description>The costmap_queue package</description>
66
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
77
<license>BSD-3-Clause</license>

‎nav2_dwb_controller/dwb_core/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>dwb_core</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>DWB core interfaces package</description>
77
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
88
<license>BSD-3-Clause</license>

‎nav2_dwb_controller/dwb_critics/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="2">
33
<name>dwb_critics</name>
4-
<version>1.3.4</version>
4+
<version>1.3.5</version>
55
<description>The dwb_critics package</description>
66
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
77
<license>BSD-3-Clause</license>

‎nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -242,7 +242,9 @@ TEST(ObstacleFootprint, LineCost)
242242
costmap_ros->getCostmap()->setCost(4, 3, 100);
243243
costmap_ros->getCostmap()->setCost(4, 4, 100);
244244

245-
ASSERT_EQ(critic->lineCost(3, 3, 0, 50), 50); // all 50
245+
auto max_y_in_grid_coordinates = costmap_ros->getCostmap()->getSizeInCellsY() - 1;
246+
ASSERT_EQ(max_y_in_grid_coordinates, 49);
247+
ASSERT_EQ(critic->lineCost(3, 3, 0, max_y_in_grid_coordinates), 50); // all 50
246248
ASSERT_EQ(critic->lineCost(4, 4, 0, 10), 100); // all 100
247249
ASSERT_EQ(critic->lineCost(0, 50, 3, 3), 100); // pass 50 and 100
248250
}

‎nav2_dwb_controller/dwb_msgs/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>dwb_msgs</name>
5-
<version>1.3.4</version>
5+
<version>1.3.5</version>
66
<description>Message/Service definitions specifically for the dwb_core</description>
77
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
88
<license>BSD-3-Clause</license>

‎nav2_dwb_controller/dwb_plugins/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="2">
33
<name>dwb_plugins</name>
4-
<version>1.3.4</version>
4+
<version>1.3.5</version>
55
<description>
66
Standard implementations of the GoalChecker
77
and TrajectoryGenerators for dwb_core

0 commit comments

Comments
 (0)