First of version of the topic-based implementation

Author: VinDp

ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp

@@ -52,6 +52,7 @@
 #include <rclcpp_action/server_goal_handle.hpp>
 #include <rclcpp/time.hpp>
 #include <rclcpp/duration.hpp>
+#include "std_msgs/msg/bool.hpp"
 
 #include "freedrive_mode_controller_parameters.hpp"
 
@@ -93,16 +94,19 @@ class FreedriveModeController : public controller_interface::ControllerInterface
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> enable_command_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> abort_command_interface_;
 
+  std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Bool>> enable_freedrive_mode_sub_;
 
   rclcpp::TimerBase::SharedPtr goal_handle_timer_;  ///< Timer to frequently check on the running goal
   rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);
+  void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg);
 
   std::shared_ptr<freedrive_mode_controller::ParamListener> freedrive_param_listener_;
   freedrive_mode_controller::Params freedrive_params_;
 
   std::atomic<bool> freedrive_active_;
   std::atomic<bool> change_requested_;
   std::atomic<double> async_state_;
+  std::atomic<double> first_log_;
 
   static constexpr double ASYNC_WAITING = 2.0;
   /**

ur_controllers/src/freedrive_mode_controller.cpp

@@ -83,6 +83,10 @@ controller_interface::CallbackReturn
 ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state)
 {
 
+  // Subscriber definition
+  enable_freedrive_mode_sub_ = get_node()->create_subscription<std_msgs::msg::Bool>(  
+      "~/freedrive_mode_active", 10,
+      std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1));
 
   const auto logger = get_node()->get_logger();
 
@@ -105,6 +109,7 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta
 {
   change_requested_ = false;
   freedrive_active_ = false;
+  first_log_ = false;
   async_state_ = std::numeric_limits<double>::quiet_NaN();
 
   {
@@ -155,6 +160,11 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S
 {
   abort_command_interface_->get().set_value(1.0);
 
+  // Set enable value to false, so in the update
+  // we can deactivate the freedrive mode
+  //Old comment?
+  freedrive_active_ = false;
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -187,13 +197,37 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat
       async_success_command_interface_->get().set_value(ASYNC_WAITING);
       async_state_ = ASYNC_WAITING;
     }
+    first_log_ = true;
     change_requested_ = false;
   }
+
+  if((async_state_ == 1.0) && (first_log_)){
+    if(freedrive_active_){
+      RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully.");
+    } else {
+      RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully.");
+    }
+    first_log_ = false;
+  }
   return controller_interface::return_type::OK;
 }
 
+void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg)
 {
   // Process the freedrive_mode command.
+  if(msg->data)
+  {
+    RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode.");
+    if((!freedrive_active_) && (!change_requested_)){
+      freedrive_active_ = true;
+      change_requested_ = true;
+    }
+  } else{
+    RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode.");
+    if((freedrive_active_) && (!change_requested_)){
+      freedrive_active_ = false;
+      change_requested_ = true;
+    }
   }
 }