diff --git a/spot_person_follower/README.md b/spot_person_follower/README.md index 18584fe5c1..7f5da33a7b 100644 --- a/spot_person_follower/README.md +++ b/spot_person_follower/README.md @@ -8,25 +8,24 @@ This demo enables Spot to follow person. This demo requires packages below. -- [spot_ros]() with [this patch](https://github.com/clearpathrobotics/spot_ros/pull/25) -- [common_msgs]() with [this patch](https://github.com/ros/common_msgs/pull/171) -- [jsk_recognition]() with [this patch](https://github.com/jsk-ros-pkg/jsk_recognition/pull/2579) and [this patch](https://github.com/jsk-ros-pkg/jsk_recognition/pull/2581) -- [jsk_spot_startup]() with [this patch](https://github.com/jsk-ros-pkg/jsk_robot/pull/1325) +- [spot_ros]() with [this branch](https://github.com/sktometometo/spot_ros) +- [common_msgs]() with [this branch](https://github.com/sktometometo/common_msgs/tree/PR/add-panorama-info) +- [jsk_recognition]() with [this branch](https://github.com/tongtybj/jsk_recognition/tree/develop/spot_trtr) +- [jsk_spot_startup]() with [this branch](https://github.com/sktometometo/jsk_robot/tree/feature/spot/dualshock4), to support dualshock4 +- [coral_usb_ros](https://github.com/knorth55/coral_usb_ros) based on an independent catkin workspace (python3) ## How to run Before running this demo, please launch and prepair a controller. -- `jsk_spot_bringup.launch` -- `object_detection_and_tracking.launch` -- `multi_object_detector.launch` +- `roslaunch jsk_spot_startup jsk_spot_bringup.launch` +- `roslaunch jsk_spot_startup object_detection_and_tracking.launch` And then, please run ```bash -roslaunch spot_person_follower demo.launch +roslaunch spot_person_follower demo.launch JOY_TOPIC:=/joy_dualshock4 ``` After this, you can start following behavior by pressing L2 button of the controller. -Spot will follow the nearest person at the time of pressing. If you want to stop the behavior, please press the L2 button again. diff --git a/spot_person_follower/launch/demo.launch b/spot_person_follower/launch/demo.launch index 35cce4ef5f..0781bd256b 100644 --- a/spot_person_follower/launch/demo.launch +++ b/spot_person_follower/launch/demo.launch @@ -1,50 +1,102 @@ - + + + + + + + + + + + - - frame_fixed: odom - frame_robot: body - duration_timeout: 0.05 - num_max_track: 20 - thresholds_distance: - '0': 5.0 # person - threshold_angle: 0.8 - threshold_lost_duration: 5.0 - threshold_move_velocity: 1.0 - threshold_tracking_switching_distance: 1.0 - threshold_target_close_distance: 0.5 - destination_offset_from_target_x: -0.5 - destination_offset_from_target_y: 0 - destination_offset_from_target_theta: 0 - publish_bbox_array: True - use_trajectory: True - rate_control: 1 - + + + + + - - - - + + + + + + + + - + + + - - - duration_timeout: 10.0 - follow_button: 6 # L2 - duration_impersity: 1.0 - + + + + + + + + + frame_fixed: "odom" + dimensions_labels: + person: [0.5, 0.5, 1.5] + + + + + + + frame_fixed: odom + frame_robot: body + duration_timeout: 0.05 + thresholds_distance: {'0': 5.0} # person + threshold_angle: 0.8 + threshold_lost_duration: 5.0 + threshold_move_velocity: 1.0 + threshold_target_close_distance: 1.0 + destination_offset_from_target_x: -1.0 # 1.5 is OK, too far? + destination_offset_from_target_y: 0 + destination_offset_from_target_theta: 0 + use_trajectory: True + rate_control: 1 + + + + + + + + + + duration_timeout: 10.0 + follow_button: 6 # L2 + duration_impersity: 1.0 + - - - + + + + diff --git a/spot_person_follower/node_scripts/follow_on_button.py b/spot_person_follower/node_scripts/follow_on_button.py index a3112a0f79..7af3c93fc3 100755 --- a/spot_person_follower/node_scripts/follow_on_button.py +++ b/spot_person_follower/node_scripts/follow_on_button.py @@ -23,13 +23,18 @@ def __init__(self): except Exception as e: rospy.logwarn('{}'.format(e)) sys.exit(1) - self._service_proxy = rospy.ServiceProxy('~follow',Trigger) + self._follow_service_proxy = rospy.ServiceProxy('~follow',Trigger) + + try: + rospy.wait_for_service('~visual_track', timeout=rospy.Duration(self._duration_timeout)) + except Exception as e: + rospy.logwarn('{}'.format(e)) + sys.exit(1) + self._track_service_proxy = rospy.ServiceProxy('~visual_track',Trigger) # self._sub_joy = rospy.Subscriber('~joy', Joy, self._callback) - rospy.loginfo('now inialized') - def _callback(self,msg): if self._follow_button < len(msg.buttons): @@ -44,11 +49,13 @@ def _callback(self,msg): # become not pressed self._is_pressed = False rospy.loginfo('button releaseed') + self.callService() else: rospy.logwarn('button id is out of range') def callService(self): - self._service_proxy() + self._follow_service_proxy() + self._track_service_proxy() if __name__ == '__main__': rospy.init_node('follow_on_button') diff --git a/spot_person_follower/node_scripts/main.py b/spot_person_follower/node_scripts/main.py index d1a3e946fa..268bc8b458 100755 --- a/spot_person_follower/node_scripts/main.py +++ b/spot_person_follower/node_scripts/main.py @@ -47,18 +47,6 @@ def __init__(self, self.last_observed = observed_time self.lock = threading.Lock() - def checkLost(self, - observed_time, - threshold_lost): - - with self.lock: - - if (observed_time - self.last_observed).to_sec() > threshold_lost: - self.state = self.LOST - return True - else: - return False - def update(self, observed_position, observed_time, @@ -110,14 +98,13 @@ def __init__(self): # parameters for multi object trackers ## maximum number of tracking objects - self._num_max_track = rospy.get_param('~num_max_track', 10) ## threshold for MOT state update self._thresholds_distance = rospy.get_param('~thresholds_distance', {}) self._threshold_angle = rospy.get_param('~threshold_angle', 0.8) self._threshold_lost = rospy.get_param('~threshold_lost_duration', 1.0) self._threshold_move = rospy.get_param('~threshold_move_velocity', 1.0) ## - self._threshold_tracking_switching_distance = rospy.get_param('~threshold_tracking_switching_distance',0.5) + ## self._threshold_target_close_distance = rospy.get_param('~threshold_target_close_distance', 0.5) ## @@ -134,8 +121,8 @@ def __init__(self): self._rate_control = rospy.get_param('~rate_control', 10.0) # members - self._lock_for_dict_objects = threading.RLock() - self._dict_objects = {} + self._lock_for_target_object = threading.RLock() + self._target_object = None # sound client self._sound_client = SoundClient(sound_action='robotsound', sound_topic='robotsound') @@ -158,39 +145,20 @@ def __init__(self): self._pub_twist_command = rospy.Publisher('~twist_command',TwistStamped,queue_size=1) # Subscriber - rate = rospy.Rate(1) - while not rospy.is_shutdown(): - try: - rospy.wait_for_message('~input_bbox_array', BoundingBoxArray, 3) - rospy.wait_for_message('~input_tracking_labels', LabelArray, 3) - break - except (rospy.ROSException, rospy.ROSInterruptException) as e: - rospy.logwarn( - 'subscribing topic seems not to be pulished. waiting... Error: {}'.format(e)) - rate.sleep() - mf_sub_bbbox_array = message_filters.Subscriber('~input_bbox_array', BoundingBoxArray) - mf_sub_tracking_labels = message_filters.Subscriber('~input_tracking_labels', LabelArray) - ts = message_filters.TimeSynchronizer([mf_sub_bbbox_array, mf_sub_tracking_labels], 10) - ts.registerCallback(self._cb_object) + self.sub_image = rospy.Subscriber(rospy.resolve_name('~input_bbox_array'), + BoundingBoxArray, self._cb_object, queue_size=1) # service server self._flag_server = False - self._lock_for_flag_server = threading.Lock() self._server_trigger = rospy.Service('~follow_person',Trigger,self._handler) - rospy.loginfo('Node is successfully initialized') - - def _cb_object(self, - msg_bbox_array, - msg_tracking_labels): + def _cb_object(self, msg): - rospy.loginfo('callback called') - - if msg_bbox_array.header.frame_id != self._frame_fixed: - rospy.logerr('frame_id of bbox (which is {}) array must be the same `~frame_fixed` which is {}'.format(msg_bbox_array.header.frame_id,self._frame_fixed)) + if msg.header.frame_id != self._frame_fixed: + rospy.logerr('frame_id of bbox (which is {}) array must be the same `~frame_fixed` which is {}'.format(msg.header.frame_id,self._frame_fixed)) return - time_observed = msg_tracking_labels.header.stamp + time_observed = msg.header.stamp try: pykdl_transform_fixed_to_robot = tf2_geometry_msgs.transform_to_kdl( @@ -205,86 +173,42 @@ def _cb_object(self, rospy.logerr('lookup transform failed. {}'.format(e)) return - with self._lock_for_dict_objects: + with self._lock_for_target_object: # add new object and update existing object state - for bbox, tracking_label in zip(msg_bbox_array.boxes, msg_tracking_labels.labels): - - if tracking_label.id not in self._dict_objects: - if len(self._dict_objects) < self._num_max_track: - self._dict_objects[tracking_label.id] = \ - TrackedObject( - bbox.label, - convert_msg_point_to_kdl_vector(bbox.pose.position), - time_observed - ) - else: - rospy.logwarn('number of objects exceeds max track. dropped.') - else: - self._dict_objects[tracking_label.id].update( - convert_msg_point_to_kdl_vector(bbox.pose.position), - time_observed, - pykdl_transform_fixed_to_robot.p, - self._threshold_move, - self._threshold_angle, - self._thresholds_distance[str(bbox.label)] - ) - # check if there is lost object - to_be_removed = [] - for key in self._dict_objects: - is_lost = self._dict_objects[key].checkLost( - time_observed, - self._threshold_lost - ) - if is_lost: - to_be_removed.append(key) - # remove lost object from dict - for key in to_be_removed: - self._dict_objects.pop(key) + bbox = msg.boxes[0] + + if self._target_object is None: + self._target_object = TrackedObject(bbox.label, + convert_msg_point_to_kdl_vector(bbox.pose.position), + time_observed) + + else: + self._target_object.update( + convert_msg_point_to_kdl_vector(bbox.pose.position), + time_observed, + pykdl_transform_fixed_to_robot.p, + self._threshold_move, + self._threshold_angle, + self._thresholds_distance[str(bbox.label)] + ) def _handler(self,req): - with self._lock_for_flag_server: - self._flag_server = not self._flag_server + self._flag_server = not self._flag_server + if self._flag_server: + rospy.logwarn("start follow") + else: + rospy.logwarn("stop follow") return TriggerResponse(True,'Succeeded') - def getNearestTargetID(self): - - distance = 3.0 # 3.0m以内 - target_id = None - try: - pykdl_transform_fixed_to_robot = tf2_geometry_msgs.transform_to_kdl( - self._tf_buffer.lookup_transform( - self._frame_fixed, - self._frame_robot, - rospy.Time(), - timeout=rospy.Duration(self._duration_timeout) - ) - ) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - rospy.logwarn('{}'.format(e)) - return None - - with self._lock_for_dict_objects: - for key in self._dict_objects: - position_robotbased = pykdl_transform_fixed_to_robot.Inverse() * self._dict_objects[key].position - if distance == None and target_id == None: - target_id = key - distance = position_robotbased.Norm() - else: - if position_robotbased.Norm() < distance: - target_id = key - distance = position_robotbased.Norm() - return target_id - - def followTarget(self, - target_id): + def followTarget(self): rate = rospy.Rate(self._rate_control) time_pre = rospy.Time.now() - rospy.loginfo('Began to follow a target with {}.'.format(target_id)) - self._sound_client.say('I have begun to follow a target with {}.'.format(target_id)) + rospy.loginfo('Began to follow a target.') + self._sound_client.say('I have begun to follow a target.') last_target_position_fixedbased = None @@ -294,7 +218,7 @@ def followTarget(self, time_current = rospy.Time.now() - # get cuurent robot position + # get current robot position try: pykdl_transform_fixed_to_robot = tf2_geometry_msgs.transform_to_kdl( self._tf_buffer.lookup_transform( @@ -308,7 +232,6 @@ def followTarget(self, rospy.logwarn('{}'.format(e)) continue - # if not self._flag_server: rospy.logerr('Stop following....') self._sound_client.say('Stop following....') @@ -316,27 +239,9 @@ def followTarget(self, self._pub_cmd_vel.publish(Twist()) return False - with self._lock_for_dict_objects: - if target_id not in self._dict_objects: - flag_recovery = False - if last_target_position_fixedbased is not None: - for key in self._dict_objects: - if (last_target_position_fixedbased - self._dict_objects[key].position).Norm() < self._threshold_tracking_switching_distance: - rospy.loginfo('tracking id switched from {} to {}'.format(target_id,key)) - self._sound_client.say('tracking id switched from {} to {}'.format(target_id,key)) - target_id = key - flag_recovery = True - break - - if not flag_recovery: - rospy.logerr('track id {} is lost..'.format(target_id)) - self._sound_client.say('I have lost the target with id {}'.format(target_id)) - self._action_client_go_pos.cancel_all_goals() - self._pub_cmd_vel.publish(Twist()) - return False - - last_target_position_fixedbased = self._dict_objects[target_id].position - predicted_target_object_position_fixedbased = self._dict_objects[target_id].position + (time_current - time_pre).to_sec() * self._dict_objects[target_id].velocity + with self._lock_for_target_object: + last_target_position_fixedbased = self._target_object.position + predicted_target_object_position_fixedbased = self._target_object.position + (time_current - time_pre).to_sec() * self._target_object.velocity # Comment: do we need to do prediction? # time_pre = time_current @@ -433,16 +338,12 @@ def main(): while not rospy.is_shutdown(): rospy.sleep(1) - with demo._lock_for_flag_server: - if not demo._flag_server: - continue - target_id = demo.getNearestTargetID() - if target_id is not None: - demo.followTarget(target_id) - else: - rospy.logwarn('None target') - with demo._lock_for_flag_server: - demo._flag_server = False + + if not demo._flag_server or demo._target_object is None: + continue + demo.followTarget() + + demo._flag_server = False if __name__ == '__main__': diff --git a/spot_person_follower/node_scripts/person_select.py b/spot_person_follower/node_scripts/person_select.py new file mode 100755 index 0000000000..c25d7f6a3c --- /dev/null +++ b/spot_person_follower/node_scripts/person_select.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy +import numpy as np + +from sensor_msgs.msg import Image +from jsk_recognition_msgs.msg import RectArray +from std_srvs.srv import Trigger, TriggerResponse + +class PersonSelect(object): + + def __init__(self): + + self.sub_image = rospy.Subscriber(rospy.resolve_name('~input'), + RectArray, self.rectarray_cb, queue_size=1) + self.pub_selected_rect = rospy.Publisher('~output_rect',RectArray,queue_size=1) + + # temporary method to detect following person + # TODO: please detect human pose + self.human_width_threshold = rospy.get_param('~human_width_threshold', 0.45) # w/h rate + self.human_yaw_threshold = rospy.get_param('~human_yaw_threshold', 1.0) # rad + self.flag_server = False + self.server_trigger = rospy.Service('~select_person', Trigger, self.trigger_cb) + + try: + msg_panorama_image = rospy.wait_for_message( + '~panorama_image', Image, 10) + except (rospy.ROSException, rospy.ROSInterruptException) as e: + rospy.logerr('{}'.format(e)) + sys.exit(1) + self.paranoma_width = msg_panorama_image.width + self.paranoma_height = msg_panorama_image.height + + def rectarray_cb(self, msg): + + if not self.flag_server: + return + + if self.paranoma_width is None or self.paranoma_height is None: + return + + closest_person_rect = None + max_width = 0 + for rect in msg.rects: + yaw = (self.paranoma_width / 2 - (rect.x + rect.width/2)) / float(self.paranoma_width) * 2 * np.pi + if rect.width < self.human_width_threshold * rect.height and np.abs(yaw) < self.human_yaw_threshold: + if rect.width > max_width: + max_width = rect.width + closest_person_rect = rect + + if closest_person_rect is not None: + rospy.loginfo("find a good person to follow!") + rect_msg = RectArray() + rect_msg.header = msg.header + rect_msg.rects.append(closest_person_rect) + self.pub_selected_rect.publish(rect_msg) + self.flag_server= False + + def trigger_cb(self, req): + + self.flag_server = not self.flag_server + return TriggerResponse(True,'Succeeded') + + +if __name__ == '__main__': + rospy.init_node('person_select') + person_select = PersonSelect() + rospy.spin() +