Merge branch 'develop'

Author: btalb

src/benchbot_robot_controller.py

@@ -75,7 +75,7 @@ def _to_simple_dict(data):
 
 class ControllerInstance(object):
 
-    def __init__(self, config_robot, config_env, ros_subs):
+    def __init__(self, config_robot, config_env, ros_subs, events=None):
         self.config_robot = config_robot
         self.config_env = config_env
 
@@ -84,6 +84,7 @@ def __init__(self, config_robot, config_env, ros_subs):
         self._cmds = None
         self._processes = None
         self._log_files = None
+        self._events = events
 
     def _replace_variables(self, text):
         for k, v in VARIABLES.items():
@@ -161,7 +162,8 @@ def start(self):
         # Wait until we move into a running state
         start_time = time.time()
         while not self.is_running():
-            time.sleep(0.25)
+            if self._events and self._events.wait(0.25):
+                return False
             if not self.health_check(check_running=False):
                 return False
             elif (time.time() - start_time > TIMEOUT_STARTUP and
@@ -189,9 +191,10 @@ def start_logging(self):
         ]
 
     def stop(self):
-        if not self.is_running():
-            print("Controller Instance is not running. Skipping stop.")
-            return False
+        # We could be in the process of starting, so this check is inappropriate
+        # if not self.is_running():
+        #     print("Controller Instance is not running. Skipping stop.")
+        #     return False
 
         # Stop all of the open processes & logging
         for p in self._processes:
@@ -225,7 +228,6 @@ def __init__(self, port=10000, auto_start=True):
         self.robot_address = 'http://0.0.0.0:' + str(port)
 
         self._auto_start = auto_start
-
         self.config = None
         self.config_valid = False
 
@@ -236,6 +238,10 @@ def __init__(self, port=10000, auto_start=True):
 
         self.instance = None
 
+        self.evt = event.Event()
+        for s in [signal.SIGINT, signal.SIGQUIT, signal.SIGTERM]:
+            signal.signal(s, lambda n, frame: self.evt.set())
+
         self.wipe()
 
     @staticmethod
@@ -468,18 +474,14 @@ def __selected_env():
         # Configure our server
         robot_server = pywsgi.WSGIServer(
             re.split('http[s]?://', self.robot_address)[-1], robot_flask)
-        evt = event.Event()
-        signal.signal(signal.SIGINT, evt.set)
-        signal.signal(signal.SIGQUIT, evt.set)
-        signal.signal(signal.SIGTERM, evt.set)
 
         # Run the server & start the real robot controller
         robot_server.start()
         print("\nRobot controller is now available @ '%s' ..." %
               self.robot_address)
         print("Waiting to receive valid config data...")
         while not self.config_valid:
-            if evt.wait(0.1):
+            if self.evt.wait(0.1):
                 break
 
             if self._auto_start and self.config_valid:
@@ -491,7 +493,7 @@ def __selected_env():
 
         # Wait until we get an exit signal or crash, then shut down gracefully
         while self.instance.health_check():
-            if evt.wait(0.1):
+            if self.evt.wait(0.1):
                 break
         print("\nShutting down the real robot ROS stack & exiting ...")
         robot_server.stop()
@@ -543,7 +545,8 @@ def start(self):
                 c['ros']
                 for c in self.connections.values()
                 if c['type'] == CONN_ROS_TO_API and c['ros'] != None
-            ])
+            ],
+            events=self.evt)
         self.instance.start()
 
     def stop(self):

src/robot_callbacks.py

@@ -14,12 +14,12 @@
 
 _MOVE_ANGLE_K = 3
 
-_MOVE_POSE_K_RHO = 0.75
+_MOVE_POSE_K_RHO = 1
 _MOVE_POSE_K_ALPHA = 4
 _MOVE_POSE_K_BETA = -1.5
 
-_MOVE_LINEAR_LIMITS = [-0.2, 0.5]
-_MOVE_ANGULAR_LIMIT = 0.3
+_MOVE_LINEAR_LIMITS = [-0.4, 0.8]
+_MOVE_ANGULAR_LIMIT = 0.5
 
 
 def __quat_from_SE3(pose):
@@ -127,8 +127,9 @@ def _move_to_angle(goal, publisher, controller):
     gamma = None
     vel_msg = Twist()
     hz_rate = rospy.Rate(_MOVE_HZ)
-    while not controller.instance.is_collided() and (
-            gamma is None or np.abs(gamma) > _MOVE_TOL_YAW):
+    while (not controller.evt.is_set() and
+           not controller.instance.is_collided() and
+           (gamma is None or np.abs(gamma) > _MOVE_TOL_YAW)):
         # Get latest orientation error
         current = __SE3_to_SE2(_current_pose(controller))
         gamma = __yaw_from_SE2(np.matmul(np.linalg.inv(current), g))
@@ -155,8 +156,9 @@ def _move_to_pose(goal, publisher, controller):
     rho = None
     vel_msg = Twist()
     hz_rate = rospy.Rate(_MOVE_HZ)
-    while not controller.instance.is_collided() and (rho is None or
-                                                     rho > _MOVE_TOL_DIST):
+    while (not controller.evt.is_set() and
+           not controller.instance.is_collided() and
+           (rho is None or rho > _MOVE_TOL_DIST)):
         # Get latest position error
         current = __SE3_to_SE2(_current_pose(controller))
         error = np.matmul(np.linalg.inv(current), g)