Merge pull request #8 from mklomp/pre-university

Features from the pre-university program

Author: mklomp

mirte_robot/robot.py

@@ -50,7 +50,6 @@ def __init__(self):
         # the controller is needed, it will be enabled.
         self.stop_controller_service = rospy.ServiceProxy('stop', Empty, persistent=True)
         self.start_controller_service = rospy.ServiceProxy('start', Empty, persistent=True)
-        #self.stop_controller_service()
 
         # Service for motor speed
         self.motors = {}
@@ -113,13 +112,20 @@ def __init__(self):
             for sensor in encoder_sensors:
                 self.encoder_services[sensor] = rospy.ServiceProxy('/mirte/get_encoder_' + encoder_sensors[sensor]["name"], GetEncoder, persistent=True)
 
-        # Services for keypad sensores
+        # Services for keypad sensors
         if rospy.has_param("/mirte/keypad"):
             keypad_sensors = rospy.get_param("/mirte/keypad")
             self.keypad_services = {}
             for sensor in keypad_sensors:
                 self.keypad_services[sensor] = rospy.ServiceProxy('/mirte/get_keypad_' + keypad_sensors[sensor]["name"], GetKeypad, persistent=True)
 
+        # Services for color sensors
+        if rospy.has_param("/mirte/color"):
+            color_sensors = rospy.get_param("/mirte/color")
+            self.color_services = {}
+            for sensor in color_sensors:
+                self.color_services[sensor] = rospy.ServiceProxy('/mirte/get_color_' + color_sensors[sensor]["name"], GetColor, persistent=True)
+
         self.get_pin_value_service = rospy.ServiceProxy('/mirte/get_pin_value', GetPinValue, persistent=True)
         self.set_pin_value_service = rospy.ServiceProxy('/mirte/set_pin_value', SetPinValue, persistent=True)
 
@@ -210,6 +216,19 @@ def getKeypad(self, keypad):
         value = self.keypad_services[keypad]()
         return value.data
 
+    def getColor(self, sensor):
+        """Gets the value of the color sensor.
+
+        Parameters:
+            sensor (str): The name of the sensor as defined in the configuration.
+
+        Returns:
+            {r, g, b, w}: Raw (0-65536) values per R(ed), G(reen), B(lue), and W(hite).
+        """
+
+        value = self.color_services[sensor]()
+        return {'r': value.color.color.r, 'g': value.color.color.g, 'b': value.color.color.b, 'w': value.color.color.w }
+
     def getAnalogPinValue(self, pin):
         """Gets the input value of an analog pin.
 
@@ -330,6 +349,24 @@ def setMotorSpeed(self, motor, value):
         motor = self.motor_services[motor](value)
         return motor.status
 
+    def setMotorControl(self, status):
+        """Enables/disables the motor controller. This is enabled on boot, but can
+        be disabled/enabled at runtime. This makes the ROS control node pause,
+        so it will not respond to Twist messages anymore when disabled.
+
+        Parameters:
+            status (bool): To which status the motor controller should be set.
+
+        Returns:
+            none
+        """
+
+        if (status):
+            self.start_controller_service()
+        else:
+            self.stop_controller_service()
+        return
+
     def stop(self):
         """Stops all DC motors defined in the configuration