Add services to control the clean motors of Roomba

create_driver/src/create_driver/create_driver.py

@@ -390,6 +390,17 @@ def dock(self):
     time.sleep(0.5)
     self.sci.force_seeking_dock()
 
+  def motors(self, side_brush, vacuum, main_brush):
+    """Activate/Deactivate other motors(side brush, vacuum and main brush)"""
+    data = 0
+    if side_brush:
+      data |= 0x01
+    if vacuum:
+      data |= 0x02
+    if main_brush:
+      data |= 0x04
+    self.sci.motors(*(data,))
+
 class Turtlebot(Roomba):
 
   """Represents a Turtlebot robot."""

create_node/CMakeLists.txt

@@ -22,7 +22,8 @@ add_message_files(DIRECTORY msg
 
 add_service_files(DIRECTORY srv
                   FILES SetDigitalOutputs.srv
-                        SetTurtlebotMode.srv)
+                        SetTurtlebotMode.srv
+                        SetCleanMotors.srv)
 
 generate_messages(DEPENDENCIES diagnostic_msgs
                                geometry_msgs

create_node/nodes/turtlebot_node.py

@@ -64,7 +64,7 @@
 
 from create_driver import Turtlebot, MAX_WHEEL_SPEED, DriverError
 from create_node.msg import TurtlebotSensorState, Drive, Turtle
-from create_node.srv import SetTurtlebotMode,SetTurtlebotModeResponse, SetDigitalOutputs, SetDigitalOutputsResponse
+from create_node.srv import SetTurtlebotMode,SetTurtlebotModeResponse, SetDigitalOutputs, SetDigitalOutputsResponse, SetCleanMotors, SetCleanMotorsResponse
 from create_node.diagnostics import TurtlebotDiagnostics
 import create_node.robot_types as robot_types
 from create_node.covariances import \
@@ -177,6 +177,7 @@ def _init_pubsub(self):
         self.sensor_state_pub = rospy.Publisher('~sensor_state', TurtlebotSensorState, queue_size=10)
         self.operating_mode_srv = rospy.Service('~set_operation_mode', SetTurtlebotMode, self.set_operation_mode)
         self.digital_output_srv = rospy.Service('~set_digital_outputs', SetDigitalOutputs, self.set_digital_outputs)
+        self.set_clean_motors_srv = rospy.Service('~set_clean_motors', SetCleanMotors, self.set_clean_motors)
 
         if self.drive_mode == 'twist':
             self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel)
@@ -315,6 +316,12 @@ def set_digital_outputs(self,req):
         self._set_digital_outputs(outputs)
         return SetDigitalOutputsResponse(True)
 
+    def set_clean_motors(self, req):
+        if not self.robot.sci:
+            raise Exception("Robot not connected, SCI not available")
+        self.robot.motors(req.side_brush, req.vacuum, req.main_brush)
+        return SetCleanMotorsResponse(True)
+
     def sense(self, sensor_state):
         self.sensor_handler.get_all(sensor_state)
         if self._gyro:

create_node/srv/SetCleanMotors.srv

@@ -0,0 +1,6 @@
+ # True: activate False: deactivate
+bool side_brush
+bool vacuum
+bool main_brush
+---
+bool done