Phase 1 - Waypoints

[redvinaa]

No description provided.

[SakshayMahna]

Hi Vince,
Thank you for the PR! The changes are looking perfect! I had a few questions:

1. Why did you choose to use service communication and not action based communication? One benefit I see is simplicity with web UI, any other reason?
2. You mentioned previously (correct me if I'm wrong) that we can use Behavior Trees for the complete interaction. But I see we have a complete node file. Probably the Behavior Trees would have been a more cleaner approach. Any reason we went for this, any challenges or it would have been more complex? Later on when we want to add Docking behavior that would be simpler.
3. Currently when I run the service, it says waiting for service for a small time and then starts moving the robot. Can we make it work directly without waiting?
4. I think the threshold for the robot for it go to its waypoint is a little lower, the robot takes some time to adjust at a waypoint before moving to next. How can we reduce that threshold? Is that using goal_tolerance in the YAML file?

[redvinaa]

Hi Sakshay!

1. The start/stop/... calls fit the service model more, because they're one-shot signals, not long-running. It wouldn't make sense for example for the start call to be accepted, have its own execute loop that can be canceled, etc.
2. Yes, this is a mistake on my end. When I started implementing, I thought patrolling refers to infinite-loop patrolling, in which case calling each navigate action separately would be the better way. But you're right, I'm gonna change it to use NavigateThroughPoses.
3. Yes, because when you call ros2 service call ... from the command-line, in the background it first calls waitForService and only then proceeds to the actual call. This is only for the command line, from python or the web_interface there's not gonna be such problem.
4. There's two parameters, the controller goal tolerance and the goal checker goal tolerance (which should normally be set to the same value). I'm gonna decrease it to 0.1m.

If you have any other questions, don't hesitate to ask!

[SakshayMahna]

Thank you! The comments make sense. Please make the final changes and I will merge it.

Just one thing, for point 1, I understand your reasoning behind using services. I was thinking considering the complete Patrol Behavior (everything including start, stop, pause) to be an Action Server.

Just like Nav2, we pass a goal to the robot, it keeps on providing feedback, not sure if we can pause it, but we can cancel the goal.

Similarly we consider giving a set of poses to the Patrol Action Server, we can on getting feedback (state of the robot and where robot is), we can pause, stop and all the things.

I am not entirely sure if there is a pause functionality for an Action Server, but it's fine, we can keep the current Services based behavior and think about this later. I will create an issue for this if you agree.

But for now, getting the prototype and MVP ready is more important.

[redvinaa]

Afaik there is no such functionality in Nav2 (or in ROS2). I'm also not sure how it would fit into the action model. Here is the discussion thread when the BT node solution was chosen instead.

I think the current implementation (which uses services) is actually the way to go because of a handful of reasons. It fits well with the web_interface node (REST api calls resemble services more than actions), it's simple to understand and is already implemented (though not on Humble, but it's easy to backport).

[SakshayMahna]

I agree services make sense for the web interface layer. But for robot behavior, we should align with Nav2’s Behavior Tree–based design so we can support extensibility (retries, conditions, recovery, etc.).

We should keep services as triggers from the web layer, but move the core patrol logic into a BT. If something is not available out of the box, we can implement it as a custom BT plugin: the intended extension mechanism in Nav2.

Can you refactor the current implementation toward this structure? Happy to discuss the BT design if needed.

[SakshayMahna]

Hi Vince,
Is it ready to be tested? I tried to build the docker image, but getting this error in the colcon build instruction:

/home/ubuntu/Desktop/ros2_ws/src/patrol_bt_plugins/src/control/pause_resume_controller.cpp: In constructor ‘patrol_bt_plugins::PauseResumeController::PauseResumeController(const string&, const NodeConfiguration&)’:
/home/ubuntu/Desktop/ros2_ws/src/patrol_bt_plugins/src/control/pause_resume_controller.cpp:29:46: error: no matching function for call to ‘rclcpp::Node::create_service<patrol_bt_plugins::Trigger>(std::string&, std::_Bind_helper<false, void (patrol_bt_plugins::PauseResumeController::*)(std::shared_ptr<std_srvs::srv::Trigger_Request_<std::allocator<void> > >, std::shared_ptr<std_srvs::srv::Trigger_Response_<std::allocator<void> > >), patrol_bt_plugins::PauseResumeController*, const std::_Placeholder<1>&, const std::_Placeholder<2>&>::type, rclcpp::ServicesQoS, rclcpp::CallbackGroup::SharedPtr&)’
   29 |   pause_srv_ = node_->create_service<Trigger>(
      |                ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
   30 |     pause_service_name,
      |     ~~~~~~~~~~~~~~~~~~~                       
   31 |     std::bind(&PauseResumeController::pause_service_callback, this, _1, _2),
      |     ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   32 |     rclcpp::ServicesQoS(), cb_group_);
      |     ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~         
In file included from /opt/ros/humble/include/rclcpp/rclcpp/executors/single_threaded_executor.hpp:28,
                 from /opt/ros/humble/include/rclcpp/rclcpp/executors.hpp:22,
                 from /opt/ros/humble/include/rclcpp/rclcpp/rclcpp.hpp:155,
                 from /home/ubuntu/Desktop/ros2_ws/src/patrol_bt_plugins/include/patrol_bt_plugins/control/pause_resume_controller.hpp:10,
                 from /home/ubuntu/Desktop/ros2_ws/src/patrol_bt_plugins/src/control/pause_resume_controller.cpp:4:
/opt/ros/humble/include/rclcpp/rclcpp/node.hpp:267:3: note: candidate: ‘template<class ServiceT, class CallbackT> typename rclcpp::Service<ServiceT>::SharedPtr rclcpp::Node::create_service(const string&, CallbackT&&, const rmw_qos_profile_t&, rclcpp::CallbackGroup::SharedPtr)’
  267 |   create_service(
      |   ^~~~~~~~~~~~~~
/opt/ros/humble/include/rclcpp/rclcpp/node.hpp:267:3: note:   template argument deduction/substitution failed:
/home/ubuntu/Desktop/ros2_ws/src/patrol_bt_plugins/src/control/pause_resume_controller.cpp:32:13: note:   cannot convert ‘rclcpp::ServicesQoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_services_default))’ (type ‘rclcpp::ServicesQoS’) to type ‘const rmw_qos_profile_t&’ {aka ‘const rmw_qos_profile_s&’}
   32 |     rclcpp::ServicesQoS(), cb_group_);
      |             ^~~~~~~~~~~~~
/home/ubuntu/Desktop/ros2_ws/src/patrol_bt_plugins/src/control/pause_resume_controller.cpp:36:47: error: no matching function for call to ‘rclcpp::Node::create_service<patrol_bt_plugins::Trigger>(std::string&, std::_Bind_helper<false, void (patrol_bt_plugins::PauseResumeController::*)(std::shared_ptr<std_srvs::srv::Trigger_Request_<std::allocator<void> > >, std::shared_ptr<std_srvs::srv::Trigger_Response_<std::allocator<void> > >), patrol_bt_plugins::PauseResumeController*, const std::_Placeholder<1>&, const std::_Placeholder<2>&>::type, rclcpp::ServicesQoS, rclcpp::CallbackGroup::SharedPtr&)’
   36 |   resume_srv_ = node_->create_service<Trigger>(
      |                 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
   37 |     resume_service_name,
      |     ~~~~~~~~~~~~~~~~~~~~                       
   38 |     std::bind(&PauseResumeController::resume_service_callback, this, _1, _2),
      |     ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   39 |     rclcpp::ServicesQoS(), cb_group_);
      |     ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~          
In file included from /opt/ros/humble/include/rclcpp/rclcpp/executors/single_threaded_executor.hpp:28,
                 from /opt/ros/humble/include/rclcpp/rclcpp/executors.hpp:22,
                 from /opt/ros/humble/include/rclcpp/rclcpp/rclcpp.hpp:155,
                 from /home/ubuntu/Desktop/ros2_ws/src/patrol_bt_plugins/include/patrol_bt_plugins/control/pause_resume_controller.hpp:10,
                 from /home/ubuntu/Desktop/ros2_ws/src/patrol_bt_plugins/src/control/pause_resume_controller.cpp:4:
/opt/ros/humble/include/rclcpp/rclcpp/node.hpp:267:3: note: candidate: ‘template<class ServiceT, class CallbackT> typename rclcpp::Service<ServiceT>::SharedPtr rclcpp::Node::create_service(const string&, CallbackT&&, const rmw_qos_profile_t&, rclcpp::CallbackGroup::SharedPtr)’
  267 |   create_service(
      |   ^~~~~~~~~~~~~~
/opt/ros/humble/include/rclcpp/rclcpp/node.hpp:267:3: note:   template argument deduction/substitution failed:
/home/ubuntu/Desktop/ros2_ws/src/patrol_bt_plugins/src/control/pause_resume_controller.cpp:39:13: note:   cannot convert ‘rclcpp::ServicesQoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_services_default))’ (type ‘rclcpp::ServicesQoS’) to type ‘const rmw_qos_profile_t&’ {aka ‘const rmw_qos_profile_s&’}
   39 |     rclcpp::ServicesQoS(), cb_group_);
      |             ^~~~~~~~~~~~~
gmake[2]: *** [CMakeFiles/pause_resume_controller_bt_node.dir/build.make:76: CMakeFiles/pause_resume_controller_bt_node.dir/src/control/pause_resume_controller.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:139: CMakeFiles/pause_resume_controller_bt_node.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< patrol_bt_plugins [5.58s, exited with code 2]

Summary: 5 packages finished [5.85s]
  1 package failed: patrol_bt_plugins
  1 package had stderr output: patrol_bt_plugins

[redvinaa]

Hi, now it should work, could you please test it?