Delete servo_on_off action. We should use servo_on_off_real_interface…

… action
iory · Nov 1, 2024 · 6ef9de9 · 6ef9de9
1 parent 4c41541
commit 6ef9de9
Show file tree

Hide file tree

Showing 4 changed files with 2 additions and 27 deletions.
diff --git a/ros/kxr_controller/include/kxr_controller/kxr_robot_hardware.h b/ros/kxr_controller/include/kxr_controller/kxr_robot_hardware.h
@@ -6,7 +6,6 @@
 #include <controller_manager/controller_manager.h>
 #include <actionlib/client/simple_action_client.h>
 #include <actionlib/server/simple_action_server.h>
-#include <kxr_controller/ServoOnOffAction.h>
 
 
 namespace kxr_controller {
@@ -29,9 +28,6 @@ namespace kxr_controller {
     ros::NodeHandle nh_;
 
   private:
-    std::shared_ptr<ServoOnOffActionServer> servo_on_off_action_server_;
-    std::shared_ptr<ServoOnOffClient> servo_on_off_action_client_;
-
     hardware_interface::JointStateInterface joint_state_interface;
     hardware_interface::PositionJointInterface joint_position_interface;
     hardware_interface::VelocityJointInterface joint_velocity_interface;
@@ -54,7 +50,6 @@ namespace kxr_controller {
     std::vector<double> joint_state_velocity_;
     std::vector<double> joint_state_effort_;
     void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg);
-    void execute_servo_on_off_callback(const kxr_controller::ServoOnOffGoalConstPtr &goal);
     boost::mutex mutex_;
   };
 }  // end of namespace kxr_controller
diff --git a/ros/kxr_controller/src/kxr_robot_hardware.cpp b/ros/kxr_controller/src/kxr_robot_hardware.cpp
@@ -82,14 +82,6 @@ namespace kxr_controller {
     joint_state_sub_ = nh_.subscribe<sensor_msgs::JointState>(clean_namespace + "/current_joint_states", 1, &KXRRobotHW::jointStateCallback, this);
     joint_command_pub_ = nh_.advertise<sensor_msgs::JointState>(clean_namespace + "/command_joint_state", 1);
     joint_velocity_command_pub_ = nh_.advertise<sensor_msgs::JointState>(clean_namespace + "/velocity_command_joint_state", 1);
-
-    ROS_INFO("Waiting for action server to start.");
-    servo_on_off_action_server_ = std::make_shared<ServoOnOffActionServer>(robot_hw_nh, clean_namespace + "/fullbody_controller/servo_on_off",
-                                                                           boost::bind(&KXRRobotHW::execute_servo_on_off_callback, this, _1), false);
-    servo_on_off_action_client_ = std::make_shared<ServoOnOffClient>(clean_namespace + "/fullbody_controller/servo_on_off_real_interface", true);
-    servo_on_off_action_client_->waitForServer();
-    servo_on_off_action_server_->start();
-    ROS_INFO("Action server started, client can send goal.");
     return true;
   }
 
@@ -148,12 +140,4 @@ namespace kxr_controller {
     joint_state_received_ = true;
   }
 
-  void KXRRobotHW::execute_servo_on_off_callback(const kxr_controller::ServoOnOffGoalConstPtr &goal)
-  {
-    boost::mutex::scoped_lock lock(mutex_);
-    servo_on_off_action_client_->sendGoal(*goal);
-    kxr_controller::ServoOnOffResult result;
-    servo_on_off_action_server_->setSucceeded(result);
-  }
-
 }  // namespace kxr_controller
diff --git a/ros/kxr_controller/test/kxr_controller.test b/ros/kxr_controller/test/kxr_controller.test
@@ -24,8 +24,6 @@
           timeout_2: 30
           topic_3: fullbody_controller/servo_on_off_real_interface/status
           timeout_3: 30
-          topic_4: fullbody_controller/servo_on_off/status
-          timeout_4: 30
         </rosparam>
     </test>
 
@@ -57,8 +55,6 @@
         timeout_2: 30
         topic_3: fullbody_controller/servo_on_off_real_interface/status
         timeout_3: 30
-        topic_4: fullbody_controller/servo_on_off/status
-        timeout_4: 30
       </rosparam>
     </test>
 

diff --git a/ros/kxreus/euslisp/kxr-interface.l b/ros/kxreus/euslisp/kxr-interface.l
@@ -68,8 +68,8 @@
                 args)
 
    (setq servo-on-off-client (instance ros::simple-action-client :init
-                                       (if namespace (format nil "/~A/~A/servo_on_off" namespace controller-name)
-                                           (format nil "/~A/servo_on_off" controller-name))
+                                       (if namespace (format nil "/~A/~A/servo_on_off_real_interface" namespace controller-name)
+                                           (format nil "/~A/servo_on_off_real_interface" controller-name))
                                        kxr_controller::ServoOnOffAction
                                        :groupname groupname))
    (setq stretch-client (instance ros::simple-action-client :init