Removed legacy from action server version

Author: VinDp

ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp

@@ -53,7 +53,6 @@
 #include <rclcpp/time.hpp>
 #include <rclcpp/duration.hpp>
 
-#include <ur_msgs/action/enable_freedrive_mode.hpp>
 #include "freedrive_mode_controller_parameters.hpp"
 
 namespace ur_controllers
@@ -94,34 +93,13 @@ 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_;
 
-  // Everything related to the RT action server
-  using FreedriveModeAction = ur_msgs::action::EnableFreedriveMode;
-  using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FreedriveModeAction>;
-  using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
-  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
 
-  RealtimeGoalHandleBuffer rt_active_goal_;         ///< Currently active action goal, if any.
   rclcpp::TimerBase::SharedPtr goal_handle_timer_;  ///< Timer to frequently check on the running goal
-  realtime_tools::RealtimeBuffer<std::unordered_map<std::string, size_t>> joint_trajectory_mapping_;
   rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);
 
-  rclcpp_action::Server<ur_msgs::action::EnableFreedriveMode>::SharedPtr freedrive_mode_action_server_;
-  rclcpp_action::GoalResponse
-  goal_received_callback(const rclcpp_action::GoalUUID& uuid,
-                         std::shared_ptr<const ur_msgs::action::EnableFreedriveMode::Goal> goal);
-
-  rclcpp_action::CancelResponse goal_cancelled_callback(
-      const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::EnableFreedriveMode>> goal_handle);
-
-  void goal_accepted_callback(
-      std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::EnableFreedriveMode>> goal_handle);
-
   std::shared_ptr<freedrive_mode_controller::ParamListener> freedrive_param_listener_;
   freedrive_mode_controller::Params freedrive_params_;
 
-  /* Start an action server with an action called: /freedrive_mode_controller/start_freedrive_mode. */
-  void start_action_server(void);
-
   std::atomic<bool> freedrive_active_;
   std::atomic<bool> change_requested_;
   std::atomic<double> async_state_;

ur_controllers/src/freedrive_mode_controller.cpp

@@ -83,7 +83,6 @@ controller_interface::CallbackReturn
 ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state)
 {
 
-  start_action_server();
 
   const auto logger = get_node()->get_logger();
 
@@ -101,17 +100,6 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St
   return ControllerInterface::on_configure(previous_state);
 }
 
-void FreedriveModeController::start_action_server(void)
-{
-  freedrive_mode_action_server_ = rclcpp_action::create_server<ur_msgs::action::EnableFreedriveMode>(
-      get_node(), std::string(get_node()->get_name()) + "/freedrive_mode",
-      std::bind(&FreedriveModeController::goal_received_callback, this, std::placeholders::_1,
-                std::placeholders::_2),
-      std::bind(&FreedriveModeController::goal_cancelled_callback, this, std::placeholders::_1),
-      std::bind(&FreedriveModeController::goal_accepted_callback, this, std::placeholders::_1));
-  return;
-}
-
 controller_interface::CallbackReturn
 ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::State& state)
 {
@@ -167,20 +155,12 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S
 {
   abort_command_interface_->get().set_value(1.0);
 
-  const auto active_goal = *rt_active_goal_.readFromRT();
-  if (active_goal) {
-    std::shared_ptr<ur_msgs::action::EnableFreedriveMode::Result> result =
-        std::make_shared<ur_msgs::action::EnableFreedriveMode::Result>();
-    active_goal->setAborted(result);
-    rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
-  }
   return CallbackReturn::SUCCESS;
 }
 
 controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/,
                                                                               const rclcpp::Duration& /*period*/)
 {
-  const auto active_goal = *rt_active_goal_.readFromRT();
   async_state_ = async_success_command_interface_->get().get_value();
 
   if(change_requested_) {
@@ -190,9 +170,6 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat
       if (!std::isnan(abort_command_interface_->get().get_value()) &&
           abort_command_interface_->get().get_value() == 1.0) {
         RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting action.");
-        std::shared_ptr<ur_msgs::action::EnableFreedriveMode::Result> result =
-            std::make_shared<ur_msgs::action::EnableFreedriveMode::Result>();
-        active_goal->setAborted(result);
         freedrive_active_ = false;
         return controller_interface::return_type::OK;
       } else {
@@ -215,26 +192,13 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat
   return controller_interface::return_type::OK;
 }
 
-rclcpp_action::GoalResponse FreedriveModeController::goal_received_callback(
-    const rclcpp_action::GoalUUID& /*uuid*/,
-    std::shared_ptr<const ur_msgs::action::EnableFreedriveMode::Goal> goal)
 {
-  RCLCPP_INFO(get_node()->get_logger(), "Received new request for freedrive mode activation.");
-
-  // Precondition: Running controller
-  if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
-    RCLCPP_ERROR(get_node()->get_logger(), "Can't enable freedrive mode. Freedrive mode controller is not running.");
-    return rclcpp_action::GoalResponse::REJECT;
+  // Process the freedrive_mode command.
   }
-
-  if (freedrive_active_) {
-    RCLCPP_ERROR(get_node()->get_logger(), "Freedrive mode is already enabled: ignoring new request.");
-    return rclcpp_action::GoalResponse::REJECT;
-  }
-
-  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }
 
+// Timeout handling for the topic
+/*
 rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback(
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::EnableFreedriveMode>> goal_handle)
 {
@@ -268,7 +232,10 @@ rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback(
   }
   return rclcpp_action::CancelResponse::ACCEPT;
 }
+*/
 
+// Don't need this anymore, but logic must be reproduced in subscriber topic
+/*
 void FreedriveModeController::goal_accepted_callback(
     std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::EnableFreedriveMode>> goal_handle)
 {
@@ -311,6 +278,7 @@ void FreedriveModeController::goal_accepted_callback(
                                                      std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal));
   return;
 }
+*/
 
 bool FreedriveModeController::waitForAsyncCommand(std::function<double(void)> get_value)
 {

ur_robot_driver/launch/ur_control.launch.py

@@ -175,12 +175,8 @@ def controller_spawner(controllers, active=True):
         "joint_trajectory_controller",
         "forward_velocity_controller",
         "forward_position_controller",
-        "freedrive_mode_controller",
-    ]
-
-    controller_spawners = [controller_spawner(controllers_active)] + [
-        controller_spawner(controllers_inactive, active=False)
         "passthrough_trajectory_controller",
+        "freedrive_mode_controller",
     ]
     if activate_joint_controller.perform(context) == "true":
         controllers_active.append(initial_joint_controller.perform(context))