Add enable charging service (#47)

Interbotix · Jul 1, 2024 · 6bf08d5 · 6bf08d5
1 parent 6d535e4
commit 6bf08d5
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 0 deletions.
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
@@ -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
@@ -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) {