Merge pull request #946 from tue-robotics/rwc2019_challenge_find_my_m…

…ates

Rwc2019 challenge find my mates

challenge_find_my_mates/CMakeLists.txt

@@ -6,3 +6,7 @@ find_package(catkin REQUIRED)
 catkin_python_setup()
 
 catkin_package()
+
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_nosetests(test)
+endif()

challenge_find_my_mates/README.md

@@ -0,0 +1,18 @@
+# Challenge find my mates
+
+## How to launch
+
+hero1:
+```
+hero-start
+```
+
+hero2: (launch separate from challenge so we can verify that deps are working)
+```
+hero-challenge-find-my-mates
+```
+
+hero2:
+```
+rosrun challenge_find_my_mates challenge_find_my_mates 
+```

challenge_find_my_mates/package.xml

@@ -12,12 +12,15 @@
 
   <depend>geometry_msgs</depend>
   <depend>rospy</depend>
+  <depend>python-matplotlib</depend>
+  <depend>python-sklearn</depend>
   <depend>smach</depend>
   <depend>tf</depend>
+  <depend>cv_bridge</depend>
 
   <!-- TU/e Robotics -->
   <depend>cb_planner_msgs_srvs</depend>
   <depend>robot_skills</depend>
   <depend>robot_smach_states</depend>
-
+  <depend>robocup_knowledge</depend>
 </package>

challenge_find_my_mates/scripts/challenge_find_my_mates

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+import rospy
+
+from robot_smach_states.util import startup
+from challenge_find_my_mates.find_my_mates import setup_statemachine
+
+if __name__ == '__main__':
+    rospy.init_node('find_my_mates_exec')
+    startup(setup_statemachine, challenge_name="find_my_mates")

challenge_find_my_mates/src/challenge_find_my_mates/__init__.py

@@ -0,0 +1,2 @@
+from .locate_people import LocatePeople
+from .report_people import ReportPeople

challenge_find_my_mates/src/challenge_find_my_mates/cluster.py

@@ -0,0 +1,73 @@
+import pickle
+from collections import defaultdict
+import pprint
+import numpy as np
+
+import matplotlib.pyplot as plt
+
+from sklearn.cluster import KMeans
+
+
+def cluster_people(people_dicts, room_center, plot=False):
+    xs = [person['map_ps'].point.x for person in people_dicts]
+    ys = [person['map_ps'].point.y for person in people_dicts]
+
+    if plot:
+        plt.scatter(xs, ys, c='r')
+
+    people_pos = np.array([xs, ys]).T  # people_pos is a np.array of [(x, y)]
+
+    kmeans = KMeans(n_clusters=4, random_state=0)
+    kmeans.fit(people_pos)
+
+    # hashable_dict = tuple(people_dicts[0].items())
+    hashable_dicts = [tuple(d.items()) for d in people_dicts]  # A dict isn't hashable so can't be dict key. But a tuple can be, so we create ((k, v), (k, v), ...) tuple
+
+    # hashable_dicts2label maps elements of people_dicts to their laels
+    hashable_dicts2label = dict(zip(hashable_dicts,
+                                    kmeans.labels_))
+
+    label2hashable_dicts = defaultdict(list)
+
+    for hashable, label in sorted(hashable_dicts2label.items()):
+        label2hashable_dicts[label].append(dict(hashable))  # And here we create a normal dict again
+    # label2hashable_dicts maps cluster labels to a list of {'rgb':..., 'person_detection':..., 'map_ps':...}
+
+    # Now we need to select wich element of the cluster is closest to the room_center
+    persons_closest_to_room_center = {}
+
+    # import pdb; pdb.set_trace()
+    for label, persons in label2hashable_dicts.items():
+        # For each cluster, we want the detection that is closest to the cluster centroid/ kmeans.cluster_centers
+        closest = sorted(persons, key=lambda person: np.hypot(*(np.array([person['map_ps'].point.x,
+                                                                          person['map_ps'].point.y]) - kmeans.cluster_centers_[label])))
+        persons_closest_to_room_center[label] = closest[0]
+
+    # persons_closest_to_room_center is a map of label to a {'rgb':..., 'person_detection':..., 'map_ps':...}
+    # import pdb; pdb.set_trace()
+
+    xs2 = [person['map_ps'].point.x for person in persons_closest_to_room_center.values()]
+    ys2 = [person['map_ps'].point.y for person in persons_closest_to_room_center.values()]
+
+    if plot:
+        plt.scatter(xs2, ys2, c='b')
+        plt.show()
+
+    # locations = zip(xs2, ys2)
+
+    return persons_closest_to_room_center.values()
+
+if __name__ == "__main__":
+    import sys
+    ppl_dicts = pickle.load(open(sys.argv[1]))
+    # ppl_dicts is a list of dicts {'rgb':..., 'person_detection':..., 'map_ps':...}
+
+    clustered_ppl = cluster_people(ppl_dicts, room_center=np.array([6, 0]), plot=True)
+
+    xs2 = [person['map_ps'].point.x for person in clustered_ppl]
+    ys2 = [person['map_ps'].point.y for person in clustered_ppl]
+    locations = zip(xs2, ys2)
+    pprint.pprint(locations)
+
+    with open('/home/kmeans_output.pickle', 'w') as dumpfile:
+        pickle.dump(clustered_ppl, dumpfile)

challenge_find_my_mates/src/challenge_find_my_mates/find_my_mates.py

@@ -0,0 +1,94 @@
+import robot_smach_states as states
+import robot_smach_states.util.designators as ds
+import smach
+
+from robocup_knowledge import load_knowledge
+from challenge_find_my_mates import LocatePeople, ReportPeople
+
+challenge_knowledge = load_knowledge('challenge_find_my_mates')
+
+STARTING_POINT = challenge_knowledge.starting_point
+ROOM_ID = challenge_knowledge.room
+SEARCH_POINT = challenge_knowledge.search_point
+OPERATOR_POINT = challenge_knowledge.operator_point
+
+
+def setup_statemachine(robot):
+    sm = smach.StateMachine(outcomes=['done', 'failed', 'aborted'])
+
+    with sm:
+        smach.StateMachine.add('START_CHALLENGE_ROBUST', states.StartChallengeRobust(robot, STARTING_POINT),
+                               transitions={'Done': 'GO_TO_SEARCH_POSE',
+                                            'Aborted': 'aborted',
+                                            'Failed': 'GO_TO_SEARCH_POSE'})
+
+        smach.StateMachine.add('SAY_START',
+                               states.Say(robot, "Finding your mates, here we go!", block=False),
+                               transitions={'spoken': 'GO_TO_SEARCH_POSE'})
+
+        smach.StateMachine.add('GO_TO_SEARCH_POSE',
+                               states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=SEARCH_POINT),
+                                                         radius=0.375),
+                               transitions={'arrived': 'RISE_FOR_THE_PEOPLE',
+                                            'goal_not_defined': 'failed',
+                                            'unreachable': 'WAIT_SEARCH_POSE'})
+
+        smach.StateMachine.add('WAIT_SEARCH_POSE',
+                               states.WaitTime(robot, 5),
+                               transitions={'preempted': 'aborted',
+                                            'waited': 'GO_TO_SEARCH_POSE'})
+
+        # noinspection PyUnusedLocal
+        @smach.cb_interface(outcomes=["done"])
+        def _rise_for_the_people(userdata=None):
+            """ Resets the location hmi attempt so that each operator gets three attempts """
+            robot.arms.values()[0]._send_joint_trajectory([[0.70, -1.9, 0.0, -1.57, 0.0]])
+            robot.speech.speak("Hi there. My Name is Hero. I'm looking for the mates of my operator", block=False)
+            return "done"
+
+        smach.StateMachine.add("RISE_FOR_THE_PEOPLE",
+                               smach.CBState(_rise_for_the_people),
+                               transitions={"done": "LOCATE_PEOPLE"})
+
+        # locate all four people
+        smach.StateMachine.add('LOCATE_PEOPLE',
+                               LocatePeople(robot,
+                                            room_id=ROOM_ID
+                                            ),
+                               transitions={'done': 'RESET_FOR_DRIVING',
+                                            }
+                               )
+
+        # noinspection PyUnusedLocal
+        @smach.cb_interface(outcomes=["done"])
+        def _reset_for_driving(userdata=None):
+            """ Resets the location hmi attempt so that each operator gets three attempts """
+            robot.speech.speak("Thank you for your attention", block=False)
+            robot.arms.values()[0]._send_joint_trajectory([[0.01, -1.9, 0.0, -1.57, 0.0],  # Inspect with q0 low
+                                                           [0.01, 0.0, -1.57, -1.57, 0.0]])  # Reset
+            return "done"
+
+        smach.StateMachine.add("RESET_FOR_DRIVING",
+                               smach.CBState(_reset_for_driving),
+                               transitions={"done": "GO_BACK_TO_OPERATOR"})
+
+        # drive back to the operator to describe the mates
+        smach.StateMachine.add('GO_BACK_TO_OPERATOR',
+                               states.NavigateToWaypoint(
+                                   robot, ds.EntityByIdDesignator(robot, id=OPERATOR_POINT),
+                                   radius=0.7, look_at_designator=ds.EntityByIdDesignator(robot, id=OPERATOR_POINT)),
+                               transitions={'arrived': 'REPORT_PEOPLE',
+                                            'goal_not_defined': 'REPORT_PEOPLE',
+                                            'unreachable': 'WAIT_GO_BACK'})
+
+        smach.StateMachine.add('WAIT_GO_BACK',
+                               states.WaitTime(robot, 5),
+                               transitions={'preempted': 'aborted',
+                                            'waited': 'GO_BACK_TO_OPERATOR'})
+
+        # check how to uniquely define them  # ToDo: make this more interesting
+        smach.StateMachine.add('REPORT_PEOPLE', ReportPeople(robot),
+                               transitions={'done': 'done'}
+                               )
+
+    return sm