Skip to content

Commit c95673e

Browse files
armgitsSteveMacenski
armgits
and
SteveMacenski
authored
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 547820f commit c95673e

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::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::LifecycleNode
227238
nav2::Publisher<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::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
}
@@ -81,6 +83,11 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
8183
collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
8284
"~/collision_points_marker");
8385

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

474481
for (std::shared_ptr<Polygon> polygon : polygons_) {
475-
if (!polygon->getEnabled()) {
482+
if (!polygon->getEnabled() || !enabled_) {
476483
continue;
477484
}
478485
if (robot_action.action_type == STOP) {
@@ -499,7 +506,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
499506
}
500507
}
501508

502-
if (robot_action.polygon_name != robot_action_prev_.polygon_name) {
509+
if ((robot_action.polygon_name != robot_action_prev_.polygon_name) && enabled_) {
503510
// Report changed robot behavior
504511
notifyActionState(robot_action, action_polygon);
505512
}
@@ -652,12 +659,26 @@ void CollisionMonitor::notifyActionState(
652659
void CollisionMonitor::publishPolygons() const
653660
{
654661
for (std::shared_ptr<Polygon> polygon : polygons_) {
655-
if (polygon->getEnabled()) {
662+
if (polygon->getEnabled() || !enabled_) {
656663
polygon->publish();
657664
}
658665
}
659666
}
660667

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

663684
#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);
@@ -218,6 +226,9 @@ class Tester : public ::testing::Test
218226

219227
// Service client for setting CollisionMonitor parameters
220228
nav2::ServiceClient<rcl_interfaces::srv::SetParameters>::SharedPtr parameters_client_;
229+
230+
// Service client for toggling collision monitor
231+
nav2::ServiceClient<nav2_msgs::srv::Toggle>::SharedPtr toggle_client_;
221232
}; // Tester
222233

223234
Tester::Tester()
@@ -261,6 +272,8 @@ Tester::Tester()
261272
cm_->create_client<rcl_interfaces::srv::SetParameters>(
262273
std::string(
263274
cm_->get_name()) + "/set_parameters");
275+
276+
toggle_client_ = cm_->create_client<nav2_msgs::srv::Toggle>("~/toggle");
264277
}
265278

266279
Tester::~Tester()
@@ -763,6 +776,22 @@ bool Tester::waitFuture(
763776
return false;
764777
}
765778

779+
bool Tester::waitToggle(
780+
rclcpp::Client<nav2_msgs::srv::Toggle>::SharedFuture result_future,
781+
const std::chrono::nanoseconds & timeout)
782+
{
783+
rclcpp::Time start_time = cm_->now();
784+
while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) {
785+
std::future_status status = result_future.wait_for(10ms);
786+
if (status == std::future_status::ready) {
787+
return true;
788+
}
789+
executor_->spin_some();
790+
std::this_thread::sleep_for(10ms);
791+
}
792+
return false;
793+
}
794+
766795
bool Tester::waitActionState(const std::chrono::nanoseconds & timeout)
767796
{
768797
rclcpp::Time start_time = cm_->now();
@@ -804,6 +833,39 @@ void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray:
804833
collision_points_marker_msg_ = msg;
805834
}
806835

836+
TEST_F(Tester, testToggleService)
837+
{
838+
// Set parameters for collision monitor
839+
setCommonParameters();
840+
addPolygon("Stop", POLYGON, 1.0, "stop");
841+
addSource(SCAN_NAME, SCAN);
842+
setVectors({"Stop"}, {SCAN_NAME});
843+
844+
// Start collision monitor node
845+
cm_->start();
846+
847+
auto request = std::make_shared<nav2_msgs::srv::Toggle::Request>();
848+
849+
// Disable test
850+
request->enable = false;
851+
{
852+
auto result_future = toggle_client_->async_call(request);
853+
ASSERT_TRUE(waitToggle(result_future, 2s));
854+
}
855+
ASSERT_FALSE(cm_->isEnabled());
856+
857+
// Enable test
858+
request->enable = true;
859+
{
860+
auto result_future = toggle_client_->async_call(request);
861+
ASSERT_TRUE(waitToggle(result_future, 2s));
862+
}
863+
ASSERT_TRUE(cm_->isEnabled());
864+
865+
// Stop the collision monitor
866+
cm_->stop();
867+
}
868+
807869
TEST_F(Tester, testProcessStopSlowdownLimit)
808870
{
809871
rclcpp::Time curr_time = cm_->now();

‎nav2_msgs/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
6060
"srv/GetShapes.srv"
6161
"srv/SetRouteGraph.srv"
6262
"srv/DynamicEdges.srv"
63+
"srv/Toggle.srv"
6364
"action/AssistedTeleop.action"
6465
"action/BackUp.action"
6566
"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)