From ab00ed10f58793f45e844f14e6587e7da66ced8e Mon Sep 17 00:00:00 2001
From: Luke Schmitt <luke@trossenrobotics.com>
Date: Mon, 1 Jul 2024 12:08:58 -0500
Subject: [PATCH] Add enable charging service

---
 .../interbotix_slate_driver/slate_base.h      | 13 ++++++++++++
 .../src/slate_base.cpp                        | 21 +++++++++++++++++++
 2 files changed, 34 insertions(+)

diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.h b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.h
index 8c5e57c..36029cf 100644
--- a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.h
+++ b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.h
@@ -61,6 +61,9 @@ class SlateBase
   // Motor status service server
   ros::ServiceServer srv_motor_torque_status_;
 
+  // Set charge enable service server
+  ros::ServiceServer srv_enable_charing_;
+
   // Name of odom frame
   std::string odom_frame_name_;
 
@@ -147,6 +150,16 @@ class SlateBase
     std_srvs::SetBool::Request & req,
     std_srvs::SetBool::Response & res);
 
+  /**
+   * @brief Process incoming enable charing service request
+   * @param req Service request containing desired 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(
+    std_srvs::SetBool::Request & req,
+    std_srvs::SetBool::Response & res);
+
   /**
    * @brief Wrap angle
    * @param angle Angle to wrap in radians
diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp
index e1e82af..19c8eec 100644
--- a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp
+++ b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp
@@ -28,6 +28,11 @@ SlateBase::SlateBase(ros::NodeHandle * node_handle)
     &SlateBase::motor_torque_status_callback,
     this);
 
+  srv_enable_charing_ = node.advertiseService(
+    "enable_charging",
+    &SlateBase::enable_charing_callback,
+    this);
+
   std::string dev;
   if (!base_driver::chassisInit(dev)) {
     ROS_FATAL("Failed to initialize base.");
@@ -168,6 +173,22 @@ bool SlateBase::motor_torque_status_callback(
   return res.success;
 }
 
+bool SlateBase::enable_charing_callback(
+    std_srvs::SetBool::Request & req,
+    std_srvs::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.";
+    ROS_INFO(res.message.c_str());
+  } else {
+    res.message = "Failed to " + enabled_disabled + " charging.";
+    ROS_ERROR(res.message.c_str());
+  }
+  return res.success;
+}
+
 float SlateBase::wrap_angle(float angle)
 {
   if (angle > M_PI) {