Merge pull request #10 from mirte-robot/v0.1.1-dev

V0.1.1 dev

Author: mklomp

mirte_robot/linetrace.py

@@ -16,11 +16,19 @@
 # Global shared memory objects (TODO: check if we need shared memory, why is server working?)
 stepper = multiprocessing.Value('b', True)
 do_step = multiprocessing.Value('b', False)
+running = multiprocessing.Value('b', False)
 
-def stop_mirte():
-     process.terminate()
+# the stop be stopped when:
+# 1) the code finishes
+# 2) the user stopped the process
+# 3) the websocket connection is closed
+def stop_mirte(terminate = True):
+     global running
+     if terminate:
+        process.terminate()
+     running.value = False
 
-def load_mirte_module(stepper, do_step):
+def load_mirte_module(stepper, do_step, running):
 
     def trace_lines(frame, event, arg):
        global do_step
@@ -43,7 +51,7 @@ def traceit(frame, event, arg):
     # Send the PID to the web interface and give it some time to call strace on this process
     # to see the output of this script
     server.send_message_to_all("pid:" + str(os.getpid()))
-    time.sleep(0.1)
+    time.sleep(0.2) #TODO: let client send signal when strace is started
 
     sys.settrace(traceit)
     # rospy.init_node() for some reason needs to be called from __main__ when importing in the regular way.
@@ -61,24 +69,32 @@ def traceit(frame, event, arg):
     # Sending the linetrace 0 to the client
     server.send_message_to_all("0")
 
+    stop_mirte(False)
+
 process = multiprocessing.Process(target = load_mirte_module, args=(stepper, do_step))
 
 def start_mirte():
     global process
-    process = multiprocessing.Process(target = load_mirte_module, args=(stepper, do_step))
+    # process should already have been killed after stop, or disconnect
+    # but, just in case make sure to stop this
+    if process.is_alive():
+      process.terminate()
+    process = multiprocessing.Process(target = load_mirte_module, args=(stepper, do_step, running))
     process.start()
 
 def client_left(client, server):
     stop_mirte()
 
 def message_received(client, server, message):
-   global stepper, do_step
+   global stepper, do_step, running
+
    if message == "b": #break (pause)
       stepper.value = True
    if message == "c": #continue (play)
       stepper.value = False
-      if not process.is_alive():
+      if not running.value:
          start_mirte()
+         running.value = True
    if message == "s": #step (step)
       do_step.value = True
    if message == "e": #exit (stop)

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"], GetColorHSL, 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:
+            {h, s, l}: Hue (0-360), Saturation (0-1), Lightness.
+        """
+
+        value = self.color_services[sensor]()
+        return {'h': value.color.h, 's': value.color.s, 'l': value.color.l }
+
     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