Skip to content

Commit de00cbc

Browse files
SteveMacenskitonynajjararmgits
authored
Jazzy Sync Sept 19, 2025 1.3.9 (#5540)
* Fix dynamic param SmacPlannerLattice (#5478) * Fix SmacPlannerLattice dynamic parameter early exit Signed-off-by: Tony Najjar <[email protected]> * remove comment Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]> * Fix duplicate poses with computePlanThroughPoses (#5488) * fix-duplicate-poses Signed-off-by: Tony Najjar <[email protected]> * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Fix seg fault (#5501) * Fix segmentation fault Signed-off-by: Tony Najjar <[email protected]> * fix linting Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]> * Add a service for enabling/disabling the collision monitor (#5493) * Added std_srvs package to dependencies Signed-off-by: Abhishekh Reddy <[email protected]> * Declared service and callback for enabling/disabling collision monitor Signed-off-by: Abhishekh Reddy <[email protected]> * Declared a variable to store collision monitor enable/disable state Signed-off-by: Abhishekh Reddy <[email protected]> * Added initialization for collision monitor enable/disable service Signed-off-by: Abhishekh Reddy <[email protected]> * Implemented service callback for collision monitor enable/disable service Signed-off-by: Abhishekh Reddy <[email protected]> * Removed std_srvs package dependency Signed-off-by: Abhishekh Reddy <[email protected]> * Added Toggle interface Signed-off-by: Abhishekh Reddy <[email protected]> * Replaced Trigger interface with the new Toggle interface Signed-off-by: Abhishekh Reddy <[email protected]> * Added default initialization for enabled flag Signed-off-by: Abhishekh Reddy <[email protected]> * Fixed toggle service name Signed-off-by: Abhishekh Reddy <[email protected]> * Updated toggle logic for collision monitor Signed-off-by: Abhishekh Reddy <[email protected]> * Added a new line at the end of file Signed-off-by: Abhishekh Reddy <[email protected]> * Update nav2_collision_monitor/src/collision_monitor_node.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Abhishekh Reddy <[email protected]> * Update nav2_collision_monitor/src/collision_monitor_node.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Abhishekh Reddy <[email protected]> * Added enabled check for logging Signed-off-by: Abhishekh Reddy <[email protected]> * Added a unit test for toggle service Signed-off-by: Abhishekh Reddy <[email protected]> * Made the getter const and added a comment Signed-off-by: Abhishekh Reddy <[email protected]> * Replaced rclcpp::spin_some Signed-off-by: Abhishekh Reddy <[email protected]> --------- Signed-off-by: Abhishekh Reddy <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * bump Jazzy to 1.3.9 for release Signed-off-by: SteveMacenski <[email protected]> * Change service type for collision monitor Signed-off-by: Steve Macenski <[email protected]> * Fix backport error Signed-off-by: SteveMacenski <[email protected]> * update Signed-off-by: SteveMacenski <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]> Signed-off-by: Abhishekh Reddy <[email protected]> Signed-off-by: SteveMacenski <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Tony Najjar <[email protected]> Co-authored-by: Abhishekh Reddy <[email protected]>
1 parent 1293655 commit de00cbc

File tree

50 files changed

+171
-59
lines changed

Some content is hidden

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

50 files changed

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

‎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.8</version>
5+
<version>1.3.9</version>
66
<description>Nav2 behavior tree wrappers, nodes, and utilities</description>
77
<maintainer email="[email protected]">Michael Jeronimo</maintainer>
88
<maintainer email="[email protected]">Carlos Orduno</maintainer>

‎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.8</version>
5+
<version>1.3.9</version>
66
<description>Nav2 behavior server</description>
77
<maintainer email="[email protected]">Carlos Orduno</maintainer>
88
<maintainer email="[email protected]">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.8</version>
5+
<version>1.3.9</version>
66
<description>Bringup scripts and configurations for the Nav2 stack</description>
77
<maintainer email="[email protected]">Michael Jeronimo</maintainer>
88
<maintainer email="[email protected]">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.8</version>
5+
<version>1.3.9</version>
66
<description>Nav2 BT Navigator Server</description>
77
<maintainer email="[email protected]">Michael Jeronimo</maintainer>
88
<license>Apache-2.0</license>

‎nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333
#include "nav2_util/twist_publisher.hpp"
3434
#include "nav2_util/twist_subscriber.hpp"
3535
#include "nav2_msgs/msg/collision_monitor_state.hpp"
36+
#include "nav2_msgs/srv/toggle.hpp"
3637

3738
#include "nav2_collision_monitor/types.hpp"
3839
#include "nav2_collision_monitor/polygon.hpp"
@@ -200,6 +201,16 @@ class CollisionMonitor : public nav2_util::LifecycleNode
200201
*/
201202
void publishPolygons() const;
202203

204+
/**
205+
* @brief Enable/disable collision monitor service callback
206+
* @param request Service request
207+
* @param response Service response
208+
*/
209+
void toggleCMServiceCallback(
210+
const std::shared_ptr<rmw_request_id_t> request_header,
211+
const std::shared_ptr<nav2_msgs::srv::Toggle::Request> request,
212+
std::shared_ptr<nav2_msgs::srv::Toggle::Response> response);
213+
203214
// ----- Variables -----
204215

205216
/// @brief TF buffer
@@ -227,6 +238,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode
227238
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
228239
collision_points_marker_pub_;
229240

241+
/// @brief Enable/disable collision monitor service
242+
rclcpp::Service<nav2_msgs::srv::Toggle>::SharedPtr toggle_cm_service_;
243+
244+
/// @brief Whether collision monitor is enabled
245+
bool enabled_;
246+
230247
/// @brief Whether main routine is active
231248
bool process_active_;
232249

‎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.8</version>
5+
<version>1.3.9</version>
66
<description>Collision Monitor</description>
77
<maintainer email="[email protected]">Alexey Merzlyakov</maintainer>
88
<maintainer email="[email protected]">Steve Macenski</maintainer>

‎nav2_collision_monitor/src/collision_monitor_node.cpp‎

Lines changed: 25 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -25,12 +25,14 @@
2525

2626
#include "nav2_collision_monitor/kinematics.hpp"
2727

28+
using namespace std::placeholders;
29+
2830
namespace nav2_collision_monitor
2931
{
3032

3133
CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options)
3234
: nav2_util::LifecycleNode("collision_monitor", "", options),
33-
process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""},
35+
enabled_{true}, process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""},
3436
stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0)
3537
{
3638
}
@@ -82,6 +84,11 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
8284
collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
8385
"~/collision_points_marker", 1);
8486

87+
// Toggle service initialization
88+
toggle_cm_service_ = create_service<nav2_msgs::srv::Toggle>(
89+
"~/toggle",
90+
std::bind(&CollisionMonitor::toggleCMServiceCallback, this, _1, _2, _3));
91+
8592
nav2_util::declare_parameter_if_not_declared(
8693
node, "use_realtime_priority", rclcpp::ParameterValue(false));
8794
bool use_realtime_priority = false;
@@ -473,7 +480,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
473480
}
474481

475482
for (std::shared_ptr<Polygon> polygon : polygons_) {
476-
if (!polygon->getEnabled()) {
483+
if (!polygon->getEnabled() || !enabled_) {
477484
continue;
478485
}
479486
if (robot_action.action_type == STOP) {
@@ -500,7 +507,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
500507
}
501508
}
502509

503-
if (robot_action.polygon_name != robot_action_prev_.polygon_name) {
510+
if ((robot_action.polygon_name != robot_action_prev_.polygon_name) && enabled_) {
504511
// Report changed robot behavior
505512
notifyActionState(robot_action, action_polygon);
506513
}
@@ -653,12 +660,26 @@ void CollisionMonitor::notifyActionState(
653660
void CollisionMonitor::publishPolygons() const
654661
{
655662
for (std::shared_ptr<Polygon> polygon : polygons_) {
656-
if (polygon->getEnabled()) {
663+
if (polygon->getEnabled() || !enabled_) {
657664
polygon->publish();
658665
}
659666
}
660667
}
661668

669+
void CollisionMonitor::toggleCMServiceCallback(
670+
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
671+
const std::shared_ptr<nav2_msgs::srv::Toggle::Request> request,
672+
std::shared_ptr<nav2_msgs::srv::Toggle::Response> response)
673+
{
674+
enabled_ = request->enable;
675+
676+
std::stringstream message;
677+
message << "Collision monitor toggled " << (enabled_ ? "on" : "off") << " successfully";
678+
679+
response->success = true;
680+
response->message = message.str();
681+
}
682+
662683
} // namespace nav2_collision_monitor
663684

664685
#include "rclcpp_components/register_node_macro.hpp"

‎nav2_collision_monitor/test/collision_monitor_node_test.cpp‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -131,6 +131,11 @@ class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor
131131
}
132132
return false;
133133
}
134+
135+
bool isEnabled() const
136+
{
137+
return enabled_;
138+
}
134139
}; // CollisionMonitorWrapper
135140

136141
class Tester : public ::testing::Test
@@ -180,6 +185,9 @@ class Tester : public ::testing::Test
180185
const std::chrono::nanoseconds & timeout);
181186
bool waitActionState(const std::chrono::nanoseconds & timeout);
182187
bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout);
188+
bool waitToggle(
189+
rclcpp::Client<nav2_msgs::srv::Toggle>::SharedFuture result_future,
190+
const std::chrono::nanoseconds & timeout);
183191

184192
protected:
185193
void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg);
@@ -215,6 +223,9 @@ class Tester : public ::testing::Test
215223

216224
// Service client for setting CollisionMonitor parameters
217225
rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr parameters_client_;
226+
227+
// Service client for toggling collision monitor
228+
rclcpp::Client<nav2_msgs::srv::Toggle>::SharedPtr toggle_client_;
218229
}; // Tester
219230

220231
Tester::Tester()
@@ -249,6 +260,8 @@ Tester::Tester()
249260
cm_->create_client<rcl_interfaces::srv::SetParameters>(
250261
std::string(
251262
cm_->get_name()) + "/set_parameters");
263+
264+
toggle_client_ = cm_->create_client<nav2_msgs::srv::Toggle>("~/toggle");
252265
}
253266

254267
Tester::~Tester()
@@ -745,6 +758,22 @@ bool Tester::waitFuture(
745758
return false;
746759
}
747760

761+
bool Tester::waitToggle(
762+
rclcpp::Client<nav2_msgs::srv::Toggle>::SharedFuture result_future,
763+
const std::chrono::nanoseconds & timeout)
764+
{
765+
rclcpp::Time start_time = cm_->now();
766+
while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) {
767+
std::future_status status = result_future.wait_for(10ms);
768+
if (status == std::future_status::ready) {
769+
return true;
770+
}
771+
rclcpp::spin_some(cm_->get_node_base_interface());
772+
std::this_thread::sleep_for(10ms);
773+
}
774+
return false;
775+
}
776+
748777
bool Tester::waitActionState(const std::chrono::nanoseconds & timeout)
749778
{
750779
rclcpp::Time start_time = cm_->now();
@@ -786,6 +815,39 @@ void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray:
786815
collision_points_marker_msg_ = msg;
787816
}
788817

818+
TEST_F(Tester, testToggleService)
819+
{
820+
// Set parameters for collision monitor
821+
setCommonParameters();
822+
addPolygon("Stop", POLYGON, 1.0, "stop");
823+
addSource(SCAN_NAME, SCAN);
824+
setVectors({"Stop"}, {SCAN_NAME});
825+
826+
// Start collision monitor node
827+
cm_->start();
828+
829+
auto request = std::make_shared<nav2_msgs::srv::Toggle::Request>();
830+
831+
// Disable test
832+
request->enable = false;
833+
{
834+
auto result_future = toggle_client_->async_send_request(request);
835+
ASSERT_TRUE(waitToggle(result_future.share(), 2s));
836+
}
837+
ASSERT_FALSE(cm_->isEnabled());
838+
839+
// Enable test
840+
request->enable = true;
841+
{
842+
auto result_future = toggle_client_->async_send_request(request);
843+
ASSERT_TRUE(waitToggle(result_future.share(), 2s));
844+
}
845+
ASSERT_TRUE(cm_->isEnabled());
846+
847+
// Stop the collision monitor
848+
cm_->stop();
849+
}
850+
789851
TEST_F(Tester, testProcessStopSlowdownLimit)
790852
{
791853
rclcpp::Time curr_time = cm_->now();

‎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.8</version>
5+
<version>1.3.9</version>
66
<description>Common support functionality used throughout the navigation 2 stack</description>
77
<maintainer email="[email protected]">Carl Delsey</maintainer>
88
<license>Apache-2.0</license>

0 commit comments

Comments
 (0)