search

tue-robotics / tue_robocup

RoboCup challenge implementations
https://github.com/orgs/tue-robotics/projects/2
42 stars 12 forks source link

Robocup 2022: find my mates #1174

Open PetervDooren opened 2 years ago

PetervDooren commented 2 years ago

Current state: Stops after detecting people, because of unhashable PointStamped message.

ToDo: Going to convert PointStamped to pykdl_ros.VectorStamped. This requires making KDL Vector, Frame, etc hashable. I have found the solution for that. Once that is implemented. I can implement the conversion to VectorStamped. After which I can test again.

MatthijsBurgh commented 2 years ago 
amigo@hero2 $ rosrun challenge_find_my_mates challenge_find_my_mates hero
[INFO][/find_my_mates_exec][1651931894.514038]: Constructing robot hero
[INFO][/find_my_mates_exec][1651931894.670234]: Waiting for ROS connections
[INFO][/find_my_mates_exec][1651931895.671180]: Will wait for another 9.0 seconds
[INFO][/find_my_mates_exec][1651931896.671005]: Will wait for another 8.0 seconds
[INFO][/find_my_mates_exec][1651931897.671247]: Will wait for another 7.0 seconds
[INFO][/find_my_mates_exec][1651931898.671252]: Will wait for another 6.0 seconds
[INFO][/find_my_mates_exec][1651931899.671242]: Will wait for another 5.0 seconds
[INFO][/find_my_mates_exec][1651931900.671253]: Will wait for another 4.0 seconds
[INFO][/find_my_mates_exec][1651931901.671263]: Will wait for another 3.0 seconds
[INFO][/find_my_mates_exec][1651931902.671217]: Will wait for another 2.0 seconds
[INFO][/find_my_mates_exec][1651931903.671307]: Will wait for another 1.0 seconds
[ERROR][/find_my_mates_exec][1651931904.771464]: /hero/handle_locator/locate_handle not connected timely
[ERROR][/find_my_mates_exec][1651931905.460744]: /hero/topological_action_planner/get_plan not connected timely
[INFO][/find_my_mates_exec][1651931905.462108]: Using robot 'hero'.
[WARN][/find_my_mates_exec][1651931906.687894]: Service 'robocup_recorder_toggle' unavailable, unable to start recording.
[INFO][/find_my_mates_exec][1651931906.690796]: State machine starting in initial state 'START_CHALLENGE_ROBUST' with userdata: 
    []
[INFO][/find_my_mates_exec][1651931906.691913]: State machine starting in initial state 'NOTIFY_EBUTTON' with userdata: 
    []
[INFO][/find_my_mates_exec][1651931906.693058]: State machine starting in initial state 'CHECK_EBUTTON' with userdata: 
    []
[INFO][/find_my_mates_exec][1651931906.694135]: State machine terminating 'CHECK_EBUTTON':'released':'succeeded'
[INFO][/find_my_mates_exec][1651931906.695192]: State machine transitioning 'NOTIFY_EBUTTON':'succeeded'-->'INITIALIZE'
[WARN][/find_my_mates_exec][1651931906.696474]: Skipping reset of torso
efficient copy
[INFO][/find_my_mates_exec][1651931906.769241]: TF link between map and base_link is checked. If it takes longer than a second, probably an error. Do a restart!!!
[INFO][/find_my_mates_exec][1651931906.771920]: State machine transitioning 'INITIALIZE':'initialized'-->'INSTRUCT_WAIT_FOR_DOOR'
[INFO][/find_my_mates_exec][1651931906.773370]: 'Hi there, I will now wait until the door is opened'
[INFO][/find_my_mates_exec][1651931906.810084]: State machine transitioning 'INSTRUCT_WAIT_FOR_DOOR':'spoken'-->'WAIT_FOR_DOOR'
[INFO][/find_my_mates_exec][1651931906.811608]: Waiting for door...
[INFO][/find_my_mates_exec][1651931906.873067]: Distance to door is more than a meter
[INFO][/find_my_mates_exec][1651931906.874529]: Unregistering laser listener and clearing data
[INFO][/find_my_mates_exec][1651931906.877019]: Door is open
[INFO][/find_my_mates_exec][1651931906.878176]: State machine transitioning 'WAIT_FOR_DOOR':'open'-->'DOOR_OPEN'
[INFO][/find_my_mates_exec][1651931906.879673]: 'Door is open!'
[INFO][/find_my_mates_exec][1651931906.908124]: State machine transitioning 'DOOR_OPEN':'spoken'-->'INIT_POSE'
efficient copy
efficient copy
[INFO][/find_my_mates_exec][1651931906.940026]: Set initial pose to 1.5, 0.5, 0.0
[INFO][/find_my_mates_exec][1651931907.942653]: State machine transitioning 'INIT_POSE':'done'-->'ENTER_ROOM'
[INFO][/find_my_mates_exec][1651931907.944651]: State machine starting in initial state 'THROUGH_DOOR' with userdata: 
    []
[INFO][/find_my_mates_exec][1651931907.945798]: 'I will start my task now'
[INFO][/find_my_mates_exec][1651931907.973468]: State machine transitioning 'THROUGH_DOOR':'spoken'-->'FORCE_DRIVE_THROUGH_DOOR'
[INFO][/find_my_mates_exec][1651931912.975187]: State machine terminating 'FORCE_DRIVE_THROUGH_DOOR':'done':'done'
[INFO][/find_my_mates_exec][1651931912.977102]: State machine terminating 'ENTER_ROOM':'done':'Done'
[INFO][/find_my_mates_exec][1651931912.978659]: State machine transitioning 'START_CHALLENGE_ROBUST':'Done'-->'GO_TO_SEARCH_POSE'
[INFO][/find_my_mates_exec][1651931912.979966]: State machine starting in initial state 'START_ANALYSIS' with userdata: 
    []
[INFO][/find_my_mates_exec][1651931912.981195]: State machine transitioning 'START_ANALYSIS':'done'-->'NAVIGATE'
[INFO][/find_my_mates_exec][1651931912.982215]: State machine starting in initial state 'GET_PLAN' with userdata: 
    []
efficient copy
efficient copy
efficient copy
[INFO][/find_my_mates_exec][1651931913.146545]: 'Affirmative!'
[INFO][/find_my_mates_exec][1651931913.175399]: State machine transitioning 'GET_PLAN':'goal_ok'-->'EXECUTE_PLAN'
[INFO][/find_my_mates_exec][1651931923.958812]: State machine terminating 'EXECUTE_PLAN':'succeeded':'arrived'
[INFO][/find_my_mates_exec][1651931923.960959]: State machine transitioning 'NAVIGATE':'arrived'-->'STOP_ANALYSIS_SUCCEED'
[INFO][/find_my_mates_exec][1651931923.962614]: State machine terminating 'STOP_ANALYSIS_SUCCEED':'done':'arrived'
[INFO][/find_my_mates_exec][1651931923.963706]: State machine transitioning 'GO_TO_SEARCH_POSE':'arrived'-->'RISE_FOR_THE_PEOPLE'
[WARN][/find_my_mates_exec][1651931923.964779]: Arms should be private and therefore not be called directly. The only interface that should be used is the get_arm function. Change your code, you are invading private property!
[INFO][/find_my_mates_exec][1651931926.638729]: 'Hi there. My Name is Hero. I'm looking for the mates of my operator'
[INFO][/find_my_mates_exec][1651931926.664222]: State machine transitioning 'RISE_FOR_THE_PEOPLE':'done'-->'LOCATE_PEOPLE'
[INFO][/find_my_mates_exec][1651931926.666101]: State machine starting in initial state 'DETECT_PERSONS' with userdata: 
    []
[INFO][/find_my_mates_exec][1651931926.667448]: 'You are all looking great today! Keep looking at my camera. I like it when everybody is staring at me!'
[INFO][/find_my_mates_exec][1651931926.692659]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931929.797510]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931931.043708]: Took 1.32, we have 0 person detections now
[INFO][/find_my_mates_exec][1651931931.065835]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931933.135847]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931933.583761]: Took 0.48, we have 0 person detections now
[INFO][/find_my_mates_exec][1651931933.586964]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931935.649381]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931936.073527]: Took 0.45, we have 0 person detections now
[INFO][/find_my_mates_exec][1651931936.074930]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931938.140566]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931939.040257]: Took 0.94, we have 2 person detections now
[INFO][/find_my_mates_exec][1651931939.042667]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931941.118026]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931942.272263]: Took 1.19, we have 5 person detections now
[INFO][/find_my_mates_exec][1651931942.274262]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931944.341129]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931945.404911]: Took 1.09, we have 8 person detections now
[INFO][/find_my_mates_exec][1651931945.407603]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931947.472322]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931948.263140]: Took 0.82, we have 9 person detections now
[INFO][/find_my_mates_exec][1651931948.267594]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931950.343112]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931951.232831]: Took 0.92, we have 10 person detections now
[INFO][/find_my_mates_exec][1651931951.240829]: 'I am looking for my mates! Dippi dee doo! Pew pew!'
[INFO][/find_my_mates_exec][1651931951.279975]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931956.048367]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931956.552953]: Took 0.53, we have 11 person detections now
[INFO][/find_my_mates_exec][1651931956.554518]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931958.618710]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931959.062762]: Took 0.47, we have 11 person detections now
[INFO][/find_my_mates_exec][1651931959.063978]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931961.108616]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931961.542469]: Took 0.45, we have 11 person detections now
[INFO][/find_my_mates_exec][1651931961.546023]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931963.601103]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931964.435652]: Took 0.86, we have 13 person detections now
[INFO][/find_my_mates_exec][1651931964.438739]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931966.497149]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931967.331274]: Took 0.86, we have 15 person detections now
[INFO][/find_my_mates_exec][1651931967.335091]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931969.503331]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931970.928498]: Took 1.46, we have 18 person detections now
[INFO][/find_my_mates_exec][1651931970.932312]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931973.004563]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931973.809290]: Took 0.84, we have 19 person detections now
[INFO][/find_my_mates_exec][1651931973.811868]: 'please look at me'
efficient copy
[INFO][/find_my_mates_exec][1651931975.979358]: Received rgb, depth, cam_info
[INFO][/find_my_mates_exec][1651931976.708801]: Took 0.76, we have 20 person detections now
[INFO][/find_my_mates_exec][1651931976.713120]: Detected 20 persons
[INFO][/find_my_mates_exec][1651931976.722582]: State machine transitioning 'DETECT_PERSONS':'done'-->'DATA_ASSOCIATION_AND_SHOW_IMAGE_ON_SCREEN'
efficient copy
efficient copy
[INFO][/find_my_mates_exec][1651931976.774195]: Found 20 person detections
[INFO][/find_my_mates_exec][1651931976.778240]: 13 in room before clustering
[ERROR][/find_my_mates_exec][1651931976.805159]: InvalidUserCodeError: Could not execute state 'DATA_ASSOCIATION_AND_SHOW_IMAGE_ON_SCREEN' of type '<smach.state.CBState object at 0x7f71c436b0a0>': Traceback (most recent call last):
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state.py", line 170, in execute
    return self._cb(ud, *self._cb_args, **self._cb_kwargs)
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/util.py", line 114, in __call__
    return self._cb(*args, **kwargs)
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/locate_people.py", line 162, in _data_association_persons_and_show_image_on_screen
    person_detection_clusters = _get_clusters()
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/locate_people.py", line 156, in _get_clusters
    clusters = cluster_people(in_room_detections, np.array([6, 0]))
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/cluster.py", line 31, in cluster_people
    hashable_dicts2label = dict(zip(hashable_dicts,
TypeError: unhashable type: 'PointStamped'

[ERROR][/find_my_mates_exec][1651931976.808616]: State 'LOCATE_PEOPLE' failed to execute.
[ERROR][/find_my_mates_exec][1651931976.812181]: Robot exited with <class 'smach.exceptions.InvalidUserCodeError'>,Could not execute state 'DATA_ASSOCIATION_AND_SHOW_IMAGE_ON_SCREEN' of type '<smach.state.CBState object at 0x7f71c436b0a0>': Traceback (most recent call last):
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state.py", line 170, in execute
    return self._cb(ud, *self._cb_args, **self._cb_kwargs)
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/util.py", line 114, in __call__
    return self._cb(*args, **kwargs)
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/locate_people.py", line 162, in _data_association_persons_and_show_image_on_screen
    person_detection_clusters = _get_clusters()
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/locate_people.py", line 156, in _get_clusters
    clusters = cluster_people(in_room_detections, np.array([6, 0]))
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/cluster.py", line 31, in cluster_people
    hashable_dicts2label = dict(zip(hashable_dicts,
TypeError: unhashable type: 'PointStamped'
,<traceback object at 0x7f71c4350500>
[INFO][/find_my_mates_exec][1651931976.813628]: Torso cancelling all goals on close
[INFO][/find_my_mates_exec][1651931976.815134]: arm_center arm cancelling all goals on all arm-related ACs on close
Traceback (most recent call last):
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state.py", line 170, in execute
    return self._cb(ud, *self._cb_args, **self._cb_kwargs)
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/util.py", line 114, in __call__
    return self._cb(*args, **kwargs)
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/locate_people.py", line 162, in _data_association_persons_and_show_image_on_screen
    person_detection_clusters = _get_clusters()
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/locate_people.py", line 156, in _get_clusters
    clusters = cluster_people(in_room_detections, np.array([6, 0]))
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/cluster.py", line 31, in cluster_people
    hashable_dicts2label = dict(zip(hashable_dicts,
TypeError: unhashable type: 'PointStamped'

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/scripts/challenge_find_my_mates", line 9, in <module>
    startup(setup_statemachine, challenge_name="find_my_mates")
  File "/home/amigo/ros/noetic/system/src/robot_smach_states/src/robot_smach_states/util/startup.py", line 103, in startup
    outcome = executioner.execute()
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 381, in execute
    container_outcome = self._update_once()
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 267, in _update_once
    raise ex
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 381, in execute
    container_outcome = self._update_once()
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 269, in _update_once
    raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
smach.exceptions.InvalidUserCodeError: Could not execute state 'DATA_ASSOCIATION_AND_SHOW_IMAGE_ON_SCREEN' of type '<smach.state.CBState object at 0x7f71c436b0a0>': Traceback (most recent call last):
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/state.py", line 170, in execute
    return self._cb(ud, *self._cb_args, **self._cb_kwargs)
  File "/home/amigo/ros/noetic/system/src/smach/src/smach/util.py", line 114, in __call__
    return self._cb(*args, **kwargs)
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/locate_people.py", line 162, in _data_association_persons_and_show_image_on_screen
    person_detection_clusters = _get_clusters()
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/locate_people.py", line 156, in _get_clusters
    clusters = cluster_people(in_room_detections, np.array([6, 0]))
  File "/home/amigo/ros/noetic/system/src/challenge_find_my_mates/src/challenge_find_my_mates/cluster.py", line 31, in cluster_people
    hashable_dicts2label = dict(zip(hashable_dicts,
TypeError: unhashable type: 'PointStamped'