Merge pull request #886 from tue-robotics/fix/robust_local_planner

StartChallengeRobust wait for local planner footprint

robot_skills/src/robot_skills/base.py

@@ -116,12 +116,11 @@ def getPlan(self, position_constraint):
         try:
             resp = self._get_plan_client(pcs)
         except Exception as e:
-            rospy.logerr("Could not get plan from global planner via service call, is the global planner running?")
-            rospy.logerr(e)
+            rospy.logerr("Could not get plan from global planner via service call: {}}".format(e))
             return None
 
         if not resp.succes:
-            rospy.logerr("Global planner couldn't plan a path to the specified constraints. Are the constraints you specified valid?")
+            rospy.logerr("Global planner couldn't plan a path to the specified constraints. Are the constraints you specified valid?: {}".format(pcs))
             return None
 
         end_time = rospy.Time.now()
@@ -134,8 +133,8 @@ def getPlan(self, position_constraint):
     def checkPlan(self, plan):
         try:
             resp = self._check_plan_client(plan)
-        except:
-            rospy.logerr("Could not check plan, is the global planner running?")
+        except Exception as e:
+            rospy.logerr("Could not check plan: {}".format(e))
             return False
 
         return resp.valid
@@ -303,8 +302,8 @@ def get_location(robot_name, tf_listener):
         target_pose.header.stamp = time
         return kdl_frame_stamped_from_pose_stamped_msg(target_pose)
 
-    except (tf.LookupException, tf.ConnectivityException):
-        rospy.logerr("tf request failed!!!")
+    except (tf.LookupException, tf.ConnectivityException) as e:
+        rospy.logerr("tf request failed!, {}".format(e))
         target_pose = geometry_msgs.msg.PoseStamped(pose=geometry_msgs.msg.Pose(position=position, orientation=orientation))
         target_pose.header.frame_id = "/map"
         return kdl_frame_stamped_from_pose_stamped_msg(target_pose)

robot_skills/src/robot_skills/perception.py

@@ -103,7 +103,7 @@ def project_roi(self, roi, frame_id=None):
 
         # If necessary, transform the point
         if frame_id is not None:
-            print("Transforming roi to {}".format(frame_id))
+            rospy.loginfo("Transforming roi to {}".format(frame_id))
             result = result.projectToFrame(frame_id=frame_id, tf_listener=self.tf_listener)
 
         # Return the result
@@ -125,24 +125,26 @@ def _get_faces(self, image=None):
             image = self.get_image()
         try:
             r = self._recognize_srv(image=image)
-            rospy.loginfo('found %d face(s) in the image', len(r.recognitions))
+            rospy.loginfo('Found %d face(s) in the image', len(r.recognitions))
         except rospy.ServiceException as e:
-            rospy.logerr(e.message)
+            rospy.logerr("Can't connect to face recognition service: {}".format(e))
             r = RecognizeResponse()
+        except Exception as e:
+            rospy.logerr("Can't detect faces: {}".format(e))
         return r
 
     def learn_person(self, name='operator'):
         HEIGHT_TRESHOLD = 88
         WIDTH_TRESHOLD = 88
         try:
             image = self.get_image()
-        except:
-            rospy.logerr("Cannot get image")
+        except Exception as e:
+            rospy.logerr("Can't get image: {}".format(e))
             return False
 
         raw_recognitions = self._get_faces(image).recognitions
         recognitions = [r for r in raw_recognitions if r.roi.height > HEIGHT_TRESHOLD and r.roi.width > WIDTH_TRESHOLD]
-        rospy.loginfo('found %d valid face(s)', len(recognitions))
+        rospy.loginfo('Found %d valid face(s)', len(recognitions))
 
         if len(recognitions) != 1:
             rospy.loginfo("Too many faces: {}".format(len(recognitions)))
@@ -154,7 +156,10 @@ def learn_person(self, name='operator'):
         try:
             self._annotate_srv(image=image, annotations=[Annotation(label=name, roi=recognition.roi)])
         except rospy.ServiceException as e:
-            rospy.logerr('annotate failed: {}'.format(e))
+            rospy.logerr("Can't connect to person learning service: {}".format(e))
+            return False
+        except Exception as e:
+            rospy.logerr("Can't learn a person: {}".format(e))
             return False
 
         return True
@@ -266,7 +271,7 @@ def get_face_properties(self, faces=None, image=None):
             face_properties_response = self._face_properties_srv(faces)
             face_properties = face_properties_response.properties_array
         except Exception as e:
-            rospy.logerr(str(e))
+            rospy.logerr(e)
             return [None] * len(faces)
 
         face_log = '\n - '.join([''] + [repr(s) for s in face_properties])

robot_skills/src/robot_skills/robot_part.py

@@ -77,7 +77,8 @@ def wait_for_connections(self, timeout):
                     try:
                         connection.wait_for_service(timeout=0.01)
                         connected = True
-                    except:
+                    except Exception as e:
+                        rospy.logerr(e)
                         connected = False
                 elif isinstance(connection, rospy.Subscriber):
                     connected = connection.get_num_connections() >= 1
@@ -126,7 +127,7 @@ def create_subscriber(self, name, *args, **kwargs):
         :param name: string with the name topic to subscribe
         :param args: other args passed to rospy.Subscriber
         :param kwargs: other keyword args passed to rospy.Subscriber
-        :return: the service client
+        :return: the Subscriber
         """
         sub = rospy.Subscriber(name, *args, **kwargs)
         self._add_connection(name, sub)

robot_skills/src/robot_skills/world_model_ed.py

@@ -14,7 +14,7 @@
 import ed_sensor_integration.srv
 from ed_people_recognition_msgs.srv import EdRecognizePeople, EdRecognizePeopleRequest
 from ed_perception.srv import Classify
-from ed_gui_server.srv import GetEntityInfo
+from ed_gui_server.srv import GetEntityInfo, GetEntityInfoResponse
 from ed_navigation.srv import GetGoalConstraint
 from cb_planner_msgs_srvs.msg import PositionConstraint
 
@@ -38,7 +38,7 @@ def get_position_constraint(self, entity_id_area_name_map):
             res = self._get_constraint_srv(entity_ids=[k for k in entity_id_area_name_map],
                                            area_names=[v for k, v in entity_id_area_name_map.iteritems()])
         except Exception as e:
-            rospy.logerr(e)
+            rospy.logerr("Can't get position constraint: {}".format(e))
             return None
 
         if res.error_msg != '':
@@ -123,8 +123,8 @@ def get_closest_entity(self, type="", center_point=None, radius=0):
         try:
             center_in_map = center_point.projectToFrame("/map", self._tf_listener)
             entities = sorted(entities, key=lambda entity: entity.distance_to_2d(center_in_map.vector))
-        except:
-            rospy.logerr("Failed to sort entities")
+        except Exception as e:
+            rospy.logerr("Failed to sort entities: {}".format(e))
             return None
 
         return entities[0]
@@ -178,8 +178,8 @@ def get_closest_laser_entity(self, type="", center_point=VectorStamped(), radius
             entities = sorted(entities, key=lambda entity: entity.distance_to_2d(
                 center_point.projectToFrame("/%s/base_link" % self.robot_name,
                                             self._tf_listener).vector))  # TODO: adjust for robot
-        except:
-            print("Failed to sort entities")
+        except Exception as e:
+            rospy.logerr("Failed to sort entities: {}".format(e))
             return None
 
         return entities[0]
@@ -193,7 +193,11 @@ def get_entity(self, id, parse=True):
         return entities[0]
 
     def get_entity_info(self, id):
-        return self._ed_entity_info_query_srv(id=id, measurement_image_border=20)
+        try:
+            return self._ed_entity_info_query_srv(id=id, measurement_image_border=20)
+        except rospy.ServiceException as e:
+            rospy.logerr("Cant get entity info of id='{}': {}".format(id, e))
+            return GetEntityInfoResponse()
 
     # ----------------------------------------------------------------------------------------------------
     #                                             UPDATING
@@ -206,8 +210,6 @@ def reset(self, keep_all_shapes=True):
             rospy.logerr("Could not reset ED: {0}".format(e))
             return False
 
-        rospy.sleep(.2)
-
     # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 
     def update_entity(self, id, type=None, frame_stamped=None, flags=None, add_flags=[], remove_flags=[], action=None):
@@ -318,8 +320,8 @@ def get_closest_possible_person_entity(self, center_point=VectorStamped(), radiu
         try:
             entities = sorted(entities, key=lambda entity: entity.distance_to_2d(center_point.vector))
             rospy.logdebug("entities sorted closest to robot = {}".format(entities))
-        except:
-            rospy.logerr("Failed to sort entities")
+        except Exception as e:
+            rospy.logerr("Failed to sort entities: {}".format(e))
             return None
 
         return entities[0]

robot_skills/test/test_perception.py

@@ -29,15 +29,15 @@
     camera_center = perception.project_roi(center_unrect, "/" + robot_name + "/top_kinect/rgb_frame")
     if camera_center.vector.x() != 0.0 and camera_center.vector.y() != 0.0:
         failed_actions += ["Center pixel of sensor is not zero in camera frame"]
-except:
-    print("Projecting {} to /{}/top_kinect/rgb_frame didn't work".format(center_unrect, robot_name))
+except Exception as e:
+    print("Projecting {} to /{}/top_kinect/rgb_frame didn't work\n{}".format(center_unrect, robot_name, e))
 
 try:
     camera_top_left = perception.project_roi(topleft_unrect, "/" + robot_name + "/top_kinect/rgb_frame")
     if camera_top_left.vector.x() != 0.0 and camera_top_left.vector.y() != 0.0:
         failed_actions += ["topleft pixel of sensor is not zero in camera frame"]
-except:
-    print("Projecting {} to /{}/top_kinect/rgb_frame didn't work".format(topleft_unrect, robot_name))
+except Exception as e:
+    print("Projecting {} to /{}/top_kinect/rgb_frame didn't work\n{}".format(topleft_unrect, robot_name, e))
 
 for failed_item in failed_actions:
     rospy.logerr(failed_item)

robot_smach_states/src/robot_smach_states/startup.py

@@ -1,9 +1,10 @@
 # System
+from functools import partial
 import math
 from threading import Event
 
 # ROS
-from geometry_msgs.msg import Quaternion
+from geometry_msgs.msg import Quaternion, PolygonStamped
 import rospy
 from sensor_msgs.msg import LaserScan
 import smach
@@ -43,15 +44,15 @@ def __init__(self, robot, initial_pose, use_entry_points=False, door=True):
 
             smach.StateMachine.add("INITIALIZE",
                                    utility.Initialize(robot),
-                                   transitions={"initialized": "INSTRUCT_WAIT_FOR_DOOR" if door else "WAIT_FOR_COMPONENTS",
+                                   transitions={"initialized": "INSTRUCT_WAIT_FOR_DOOR" if door else "WAIT_FOR_LOCAL_PLANNER",
                                                 "abort": "Aborted"})
 
-            # ToDo: HACK
-            # We wait a while before starting the challenge, otherwise some software is not initialised yet.
-            smach.StateMachine.add("WAIT_FOR_COMPONENTS",
-                                   utility.WaitTime(robot, 5),
-                                   transitions={"waited": "INIT_POSE",
-                                                "preempted": "Aborted"})
+            # We wait till the  local planner is ready before starting the challenge,
+            # otherwise the robot will not drive.
+            smach.StateMachine.add("WAIT_FOR_LOCAL_PLANNER",
+                                   WaitForLocalPlanner(robot, 10),
+                                   transitions={"ready": "INIT_POSE",
+                                                "timeout": "Aborted"})
 
             smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR",
                                    human_interaction.Say(robot, ["Hi there, I will now wait until the door is opened",
@@ -239,3 +240,56 @@ def execute(self, userdata=None):
 
         rospy.loginfo("Timed out with door still closed")
         return "closed"
+
+
+class WaitForLocalPlanner(smach.State):
+    """
+    Wait till a valid footprint message from the local planner is received. A footprint is valid if it contains at least
+    3 points. If no valid message is received within the timeout, "timeout" is returned.
+    """
+    def __init__(self, robot, timeout):
+        """
+        Constructor
+        :param robot: robot object
+        :type robot: robot
+        :param timeout: timeout
+        :type timeout: (float, int)
+        """
+        smach.State.__init__(self, outcomes=["ready", "timeout"])
+        self._robot = robot
+        self._timeout = timeout
+
+    @staticmethod
+    def msg_cb(ready_event, msg):
+        # type: (Event, PolygonStamped) -> None
+        """
+        Footprint message callback
+        :param ready_event: Event to set when ready
+        :type ready_event: Event
+        :param msg: footprint message
+        :type msg: PolygonStamped
+        """
+        if len(msg.polygon.points) >= 3:
+            rospy.loginfo("Valid footprint received")
+            ready_event.set()
+
+    def execute(self, userdate=None):
+        rospy.loginfo("Waiting for local planner footprint")
+        footprint_topic = "/{}/local_planner/local_costmap/robot_footprint/footprint_stamped".\
+            format(self._robot.robot_name)
+
+        ready_event = Event()
+
+        footprint_sub = rospy.Subscriber(footprint_topic, PolygonStamped, partial(self.msg_cb, ready_event))
+
+        ready_before_timeout = ready_event.wait(self._timeout)
+
+        rospy.loginfo("Unregistering footprint subscriber")
+        footprint_sub.unregister()
+
+        if ready_before_timeout:
+            rospy.loginfo("Local Planner is ready")
+            return "ready"
+
+        rospy.loginfo("Local Planner not ready during timeout")
+        return "timeout"