Add enable charging service

interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.hpp

@@ -100,6 +100,9 @@ class SlateBase : public rclcpp::Node
   // Motor status service server
   rclcpp::Service<SetBool>::SharedPtr srv_motor_torque_status_;
 
+  // Set charge enable service server
+  rclcpp::Service<SetBool>::SharedPtr srv_enable_charing_;
+
   // Name of odom frame
   std::string odom_frame_name_;
 
@@ -190,6 +193,18 @@ class SlateBase : public rclcpp::Node
     const std::shared_ptr<SetBool::Request> req,
     const std::shared_ptr<SetBool::Response> res);
 
+  /**
+   * @brief Process incoming enable charing service request
+   * @param request_header Incoming RMW request identifier
+   * @param req Service request containing desired charing enable status
+   * @param res[out] Service response containing a success indication and a message
+   * @return true if service succeeded, false otherwise
+   */
+  bool enable_charing_callback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<SetBool::Request> req,
+    const std::shared_ptr<SetBool::Response> res);
+
   /**
    * @brief Wrap angle
    * @param angle Angle to wrap in radians

interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp

@@ -67,6 +67,10 @@ SlateBase::SlateBase(const rclcpp::NodeOptions & options)
     "set_motor_torque_status",
     std::bind(&SlateBase::motor_torque_status_callback, this, _1, _2, _3));
 
+  srv_enable_charing_ = create_service<SetBool>(
+    "enable_charging",
+    std::bind(&SlateBase::enable_charing_callback, this, _1, _2, _3));
+
   std::string dev;
   if (!base_driver::chassisInit(dev)) {
     RCLCPP_ERROR(get_logger(), "Failed to initialize base port.");
@@ -210,6 +214,23 @@ bool SlateBase::motor_torque_status_callback(
   return res->success;
 }
 
+bool SlateBase::enable_charing_callback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<SetBool::Request> req,
+  const std::shared_ptr<SetBool::Response> res)
+{
+  res->success = base_driver::setCharge(req->data);
+  std::string enabled_disabled = req->data ? "enable" : "disable";
+  if (res->success) {
+    res->message = "Successfully " + enabled_disabled + "d charging.";
+    RCLCPP_INFO(get_logger(), res->message.c_str());
+  } else {
+    res->message = "Failed to " + enabled_disabled + " charging.";
+    RCLCPP_ERROR(get_logger(), res->message.c_str());
+  }
+  return res->success;
+}
+
 float SlateBase::wrap_angle(float angle)
 {
   if (angle > M_PI) {