added servo_start and servo_stop to grpc server

Author: macmacal

aegis_grpc/CMakeLists.txt

@@ -18,12 +18,13 @@ find_package(ament_cmake REQUIRED)
 set(ROS_DEPENDS
     rclcpp
     rclcpp_action
-    controller_manager_msgs
     control_msgs
+    controller_manager_msgs
     geometry_msgs
-    sensor_msgs
     moveit_msgs
-    moveit_ros_planning_interface)
+    moveit_ros_planning_interface
+    sensor_msgs
+    std_srvs)
 
 # Hide warnings from importing moveit packages
 if(POLICY CMP0167)

aegis_grpc/include/aegis_grpc/robot_control_service.hpp

@@ -9,13 +9,14 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
-#include <controller_manager_msgs/srv/switch_controller.hpp>
 #include <control_msgs/action/gripper_command.hpp>
 #include <control_msgs/msg/joint_jog.hpp>
-#include <geometry_msgs/msg/twist.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <controller_manager_msgs/srv/switch_controller.hpp>
 #include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
 #include <moveit/move_group_interface/move_group_interface.h>
+#include <std_srvs/srv/trigger.hpp>
 
 
 namespace aegis_grpc {
@@ -84,6 +85,8 @@ class RobotControlServiceImpl final
     bool SwitchControllers(
         const std::vector<std::string>& activate,
         const std::vector<std::string>& deactivate);
+    bool CallServoStartService();
+    bool CallServoStopService();
 
     void ServoPublishLoop();
     void GripperSendGoal(double position, double max_effort);
@@ -97,6 +100,8 @@ class RobotControlServiceImpl final
     std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
 
     rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_client_;
+    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr start_servo_client_;
+    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_servo_client_;
     rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr servo_joint_pub_;
     rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr servo_tcp_pub_;
     rclcpp_action::Client<GripperCommand>::SharedPtr gripper_client_;

aegis_grpc/package.xml

@@ -27,6 +27,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>sensor_msgs</depend>
+  <depend>std_srvs</depend>
 
   <exec_depend>python3-grpc-tools</exec_depend>
   <exec_depend>python3-grpcio</exec_depend>

aegis_grpc/src/robot_control_service.cpp

@@ -36,6 +36,8 @@ RobotControlServiceImpl::RobotControlServiceImpl(
   RCLCPP_INFO(get_logger(), "> Frequency ratio for servo re-publishig is %i", servo_frequency_ratio_);
 
   switch_controller_client_ = node_->create_client<controller_manager_msgs::srv::SwitchController>("/controller_manager/switch_controller");
+  start_servo_client_ = node_->create_client<std_srvs::srv::Trigger>("/servo_node/start_servo");
+  stop_servo_client_ = node_->create_client<std_srvs::srv::Trigger>("/servo_node/stop_servo");
   servo_joint_pub_ = node_->create_publisher<control_msgs::msg::JointJog>(
       node_->get_parameter("topic_servo_joint").as_string(), 10);
   servo_tcp_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>(
@@ -101,6 +103,40 @@ bool RobotControlServiceImpl::SwitchControllers(
   return future.get()->ok;
 }
 
+bool RobotControlServiceImpl::CallServoStartService() {
+  if (!start_servo_client_->wait_for_service(action_timeout_)) {
+    RCLCPP_WARN(get_logger(), "Service `/servo_node/start_servo` not available.");
+    return false;
+  }
+
+  auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
+  auto result_future = start_servo_client_->async_send_request(request);
+
+  if (result_future.wait_for(action_timeout_) != std::future_status::ready) {
+    RCLCPP_WARN(get_logger(), "Service `/servo_node/start_servo` call timed out.");
+    return false;
+  }
+  auto result = result_future.get();
+  return result->success;
+}
+
+bool RobotControlServiceImpl::CallServoStopService() {
+  if (!stop_servo_client_->wait_for_service(action_timeout_)) {
+    RCLCPP_WARN(get_logger(), "Service `/servo_node/stop_servo` not available.");
+    return false;
+  }
+
+  auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
+  auto result_future = stop_servo_client_->async_send_request(request);
+
+  if (result_future.wait_for(action_timeout_) != std::future_status::ready) {
+    RCLCPP_WARN(get_logger(), "Service `/servo_node/stop_servo` call timed out.");
+    return false;
+  }
+  auto result = result_future.get();
+  return result->success;
+}
+
 grpc::Status RobotControlServiceImpl::ServoEnable(
   grpc::ServerContext*,
   const google::protobuf::Empty*,
@@ -117,13 +153,21 @@ grpc::Status RobotControlServiceImpl::ServoEnable(
     return grpc::Status(grpc::StatusCode::INTERNAL, err);
   }
 
+  if (!CallServoStartService()) {
+    std::string err = "Failed to start servo service.";
+    RCLCPP_WARN(get_logger(), "[RobotControlService][ServoEnable] %s", err.c_str());
+    response->set_msg(err);
+    return grpc::Status(grpc::StatusCode::INTERNAL, err);
+  }
+
   {
     std::lock_guard<std::mutex> lock(servo_mutex_);
     servo_mode_ = ServoMode::JointJog;
   }
 
   response->set_success(true);
   response->set_msg("");
+  RCLCPP_INFO(get_logger(), "[RobotControlService][ServoDisable] Servo enabled");
   return grpc::Status::OK;
 }
 
@@ -134,6 +178,13 @@ grpc::Status RobotControlServiceImpl::ServoDisable(
 {
   response->set_success(false);
 
+  if (!CallServoStopService()) {
+    std::string err = "Failed to stop servo service.";
+    RCLCPP_WARN(get_logger(), "[RobotControlService][ServoDisable] %s", err.c_str());
+    response->set_msg(err);
+    return grpc::Status(grpc::StatusCode::INTERNAL, err);
+  }
+
   if (!SwitchControllers(
         {"scaled_joint_trajectory_controller"},
         {"forward_position_controller"})) {
@@ -152,7 +203,7 @@ grpc::Status RobotControlServiceImpl::ServoDisable(
 
   response->set_success(true);
   response->set_msg("");
-  RCLCPP_WARN(get_logger(), "[RobotControlService][ServoDisable] RETURN");
+  RCLCPP_INFO(get_logger(), "[RobotControlService][ServoDisable] Servo disabled");
   return grpc::Status::OK;
 }