Skip to content

Commit 7e31b88

Browse files
SteveMacenskimini-1235georgflickBCKSELFDRIVEWORLDmattbooker
authored
Jazz sync - April 15, 2025 (#5090)
* Add short delay before lookup transform in polygon test (#4939) * Add short delay before lookup transform Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Populate stamped command message with now timestamp, if velocity timed out. (#4959) Signed-off-by: Georg Flick <georg.flick@enway.ai> * Integrate ClearCostmapExceptRegion and ClearCostmapAroundRobot Services into the API (#4962) * Add ClearCostmapExceptRegion and ClearCostmapAroundRobot services to BasicNavigator API Signed-off-by: BCKSELFDRIVEWORLD <bckselfdrive@gmail.com> * typo ament_flake8 Signed-off-by: BCKSELFDRIVEWORLD <bckselfdrive@gmail.com> * type fix ament_flake Signed-off-by: BCKSELFDRIVEWORLD <bckselfdrive@gmail.com> --------- Signed-off-by: BCKSELFDRIVEWORLD <bckselfdrive@gmail.com> * Fix urls in node hybrid (#4973) Signed-off-by: mattbooker <mattbooker97@gmail.com> * Use main function to replace global variable in gtest. (#4978) Signed-off-by: ChenYing Kuo <evshary@gmail.com> * nav2_behavior_tree: fix input port parsing error in AreErrorCodesPresent (#4986) The getInput method does not support std::set<uint16_t> parsing. So, let's replace the type of the input port by std::vector<int> which is supported, and convert the list to a std::set<uint16_t>. This commit fixes issue #4985. Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> * Change to goal checker orientation for yaw angle (#4988) - Fixed discrepancy in goal checker orientation, which was checking for < tolerance instead of <= tolerance, as all the other limit checks are. - Reduced tolerance time for the progress checker unit tests to 0.1 seconds, to reduce test runtime from ~17 to ~7 seconds. Signed-off-by: Rasmus Larsson <rasmus.larsson@accenture.com> * Declare_parameter_if_not_declared in docking navigator (#5023) Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * bumping to 1.3.6 for release Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Georg Flick <georg.flick@enway.ai> Signed-off-by: BCKSELFDRIVEWORLD <bckselfdrive@gmail.com> Signed-off-by: mattbooker <mattbooker97@gmail.com> Signed-off-by: ChenYing Kuo <evshary@gmail.com> Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> Signed-off-by: Rasmus Larsson <rasmus.larsson@accenture.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: mini-1235 <58433591+mini-1235@users.noreply.github.com> Co-authored-by: Georg Flick <54032815+georgflick@users.noreply.github.com> Co-authored-by: Burak Can Kaya <146545020+BCKSELFDRIVEWORLD@users.noreply.github.com> Co-authored-by: Matthew Booker <mattbooker97@gmail.com> Co-authored-by: ChenYing Kuo (CY) <evshary@gmail.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: RasmusLar <101558931+RasmusLar@users.noreply.github.com> Co-authored-by: Alberto Tudela <ajtudela@gmail.com>
1 parent 1a8b493 commit 7e31b88

File tree

55 files changed

+319
-100
lines changed

Some content is hidden

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

55 files changed

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

‎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/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.5</version>
5+
<version>1.3.6</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/test/plugins/condition/test_are_error_codes_present.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF
2828
void SetUp()
2929
{
3030
uint16_t error_code = ActionResult::NONE;
31-
std::set<uint16_t> error_codes_to_check = {ActionResult::UNKNOWN}; //NOLINT
31+
std::vector<int> error_codes_to_check = {ActionResult::UNKNOWN}; //NOLINT
3232
config_->blackboard->set("error_code", error_code);
3333
config_->blackboard->set("error_codes_to_check", error_codes_to_check);
3434

‎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.5</version>
5+
<version>1.3.6</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.5</version>
5+
<version>1.3.6</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/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.5</version>
5+
<version>1.3.6</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.5</version>
5+
<version>1.3.6</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_collision_monitor/test/polygons_test.cpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -753,6 +753,8 @@ TEST_F(Tester, testPolygonTopicUpdateDifferentFrame)
753753

754754
// Move BASE2_FRAME_ID to 0.2 m away from BASE_FRAME_ID
755755
sendTransforms(0.2);
756+
// sleep for a short time to make sure that the transform is published
757+
std::this_thread::sleep_for(50ms);
756758
// updatePolygon(vel) should update poly coordinates to correct ones in BASE_FRAME_ID
757759
nav2_collision_monitor::Velocity vel{0.0, 0.0, 0.0};
758760
polygon_->updatePolygon(vel);

‎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.5</version>
5+
<version>1.3.6</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.5</version>
5+
<version>1.3.6</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.5</version>
5+
<version>1.3.6</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_controller/plugins/simple_goal_checker.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -114,7 +114,7 @@ bool SimpleGoalChecker::isGoalReached(
114114
double dyaw = angles::shortest_angular_distance(
115115
tf2::getYaw(query_pose.orientation),
116116
tf2::getYaw(goal_pose.orientation));
117-
return fabs(dyaw) < yaw_goal_tolerance_;
117+
return fabs(dyaw) <= yaw_goal_tolerance_;
118118
}
119119

120120
bool SimpleGoalChecker::getTolerances(

‎nav2_controller/plugins/test/goal_checker.cpp

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
#include "nav2_controller/plugins/stopped_goal_checker.hpp"
4141
#include "nav_2d_utils/conversions.hpp"
4242
#include "nav2_util/lifecycle_node.hpp"
43+
#include "eigen3/Eigen/Geometry"
4344

4445
using nav2_controller::SimpleGoalChecker;
4546
using nav2_controller::StoppedGoalChecker;
@@ -237,6 +238,108 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params)
237238
EXPECT_EQ(pose_tol.position.y, 200.0);
238239
}
239240

241+
TEST(StoppedGoalChecker, is_reached)
242+
{
243+
auto x = std::make_shared<TestLifecycleNode>("goal_checker");
244+
245+
SimpleGoalChecker gc;
246+
StoppedGoalChecker sgc;
247+
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
248+
249+
sgc.initialize(x, "test", costmap);
250+
gc.initialize(x, "test2", costmap);
251+
geometry_msgs::msg::Pose goal_pose;
252+
geometry_msgs::msg::Twist velocity;
253+
geometry_msgs::msg::Pose current_pose;
254+
255+
// Current linear x position is tolerance away from goal
256+
current_pose.position.x = 0.25;
257+
velocity.linear.x = 0.25;
258+
EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity));
259+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
260+
sgc.reset();
261+
gc.reset();
262+
263+
// Current linear x speed exceeds tolerance
264+
velocity.linear.x = 0.25 + std::numeric_limits<double>::epsilon();
265+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
266+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
267+
sgc.reset();
268+
gc.reset();
269+
270+
// Current linear x position is further than tolerance away from goal
271+
current_pose.position.x = 0.25 + std::numeric_limits<double>::epsilon();
272+
velocity.linear.x = 0.25;
273+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
274+
EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity));
275+
sgc.reset();
276+
gc.reset();
277+
current_pose.position.x = 0.0;
278+
velocity.linear.x = 0.0;
279+
280+
// Current linear position is tolerance away from goal
281+
current_pose.position.x = 0.25 / std::sqrt(2);
282+
current_pose.position.y = 0.25 / std::sqrt(2);
283+
velocity.linear.x = 0.25 / std::sqrt(2);
284+
velocity.linear.y = 0.25 / std::sqrt(2);
285+
EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity));
286+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
287+
sgc.reset();
288+
gc.reset();
289+
290+
// Current linear speed exceeds tolerance
291+
velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits<double>::epsilon();
292+
velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits<double>::epsilon();
293+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
294+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
295+
sgc.reset();
296+
gc.reset();
297+
298+
// Current linear position is further than tolerance away from goal
299+
current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits<double>::epsilon();
300+
current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits<double>::epsilon();
301+
velocity.linear.x = 0.25 / std::sqrt(2);
302+
velocity.linear.y = 0.25 / std::sqrt(2);
303+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
304+
EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity));
305+
sgc.reset();
306+
gc.reset();
307+
308+
current_pose.position.x = 0.0;
309+
velocity.linear.x = 0.0;
310+
311+
312+
// Current angular position is tolerance away from goal
313+
auto quat =
314+
(Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs();
315+
// epsilon for orientation is a lot bigger than double limit, probably from TF getYaw
316+
auto quat_epsilon =
317+
(Eigen::AngleAxisd::Identity() *
318+
Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs();
319+
320+
current_pose.orientation.z = quat[2];
321+
current_pose.orientation.w = quat[3];
322+
velocity.angular.z = 0.25;
323+
EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity));
324+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
325+
sgc.reset();
326+
gc.reset();
327+
328+
// Current angular speed exceeds tolerance
329+
velocity.angular.z = 0.25 + std::numeric_limits<double>::epsilon();
330+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
331+
EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity));
332+
sgc.reset();
333+
gc.reset();
334+
335+
// Current angular position is further than tolerance away from goal
336+
current_pose.orientation.z = quat_epsilon[2];
337+
current_pose.orientation.w = quat_epsilon[3];
338+
velocity.angular.z = 0.25;
339+
EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity));
340+
EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity));
341+
}
342+
240343
int main(int argc, char ** argv)
241344
{
242345
rclcpp::init(argc, argv);

0 commit comments

Comments
 (0)