Skip to content

Commit 5ecb60c

Browse files
armgitsSteveMacenski
armgits
and
SteveMacenski
committed
Add a service for enabling/disabling the collision monitor (#5493)
* Added std_srvs package to dependencies Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Declared service and callback for enabling/disabling collision monitor Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Declared a variable to store collision monitor enable/disable state Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Added initialization for collision monitor enable/disable service Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Implemented service callback for collision monitor enable/disable service Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Removed std_srvs package dependency Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Added Toggle interface Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Replaced Trigger interface with the new Toggle interface Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Added default initialization for enabled flag Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Fixed toggle service name Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Updated toggle logic for collision monitor Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Added a new line at the end of file Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Update nav2_collision_monitor/src/collision_monitor_node.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Update nav2_collision_monitor/src/collision_monitor_node.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Added enabled check for logging Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Added a unit test for toggle service Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Made the getter const and added a comment Signed-off-by: Abhishekh Reddy <helloarm@pm.me> * Replaced rclcpp::spin_some Signed-off-by: Abhishekh Reddy <helloarm@pm.me> --------- Signed-off-by: Abhishekh Reddy <helloarm@pm.me> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
1 parent a923648 commit 5ecb60c

File tree

5 files changed

+109
-4
lines changed

5 files changed

+109
-4
lines changed

‎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+
nav2::ServiceServer<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/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+
executor_->spin_some();
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_call(request);
835+
ASSERT_TRUE(waitToggle(result_future, 2s));
836+
}
837+
ASSERT_FALSE(cm_->isEnabled());
838+
839+
// Enable test
840+
request->enable = true;
841+
{
842+
auto result_future = toggle_client_->async_call(request);
843+
ASSERT_TRUE(waitToggle(result_future, 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_msgs/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
4848
"srv/ReloadDockDatabase.srv"
4949
"srv/SetRouteGraph.srv"
5050
"srv/DynamicEdges.srv"
51+
"srv/Toggle.srv"
5152
"action/AssistedTeleop.action"
5253
"action/BackUp.action"
5354
"action/ComputePathToPose.action"

‎nav2_msgs/srv/Toggle.srv‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
bool enable
2+
---
3+
bool success
4+
string message

0 commit comments

Comments
 (0)