def print_data(userdata): res = userdata.ttop_data[0] print res if not recognize_objects: pub = res.pose # If the recognize_objects parameter is False z = res.pose.pose.position.z # Height of the table minx = res.x_min miny = res.y_min maxx = res.x_max maxy = res.y_max header = res.pose.header else: pub = res.table.pose # If the recognize_objects parameter is True z = res.table.pose.pose.position.z # Height of the table minx = res.x_min miny = res.y_min maxx = res.x_max maxy = res.y_max header = res.table.pose.header pub.pose = transform_pose(pub.pose, pub.header.frame_id, 'base_link', timeout=3) #Test with min and max values ps = PoseStamped() ps.header = header #Select one to publish: #ps.pose = Pose(Point(minx, miny, z), Quaternion()) # Down right (from the back view of the robot) #ps.pose = Pose(Point(minx, maxy, z), Quaternion()) # Down left (from the back view of the robot) #ps.pose = Pose(Point(maxx, miny, z), Quaternion()) # Up right (from the back view of the robot) #ps.pose = Pose(Point(maxx, maxy, z), Quaternion()) # Up left (from the back view of the robot) #ps.pose = Pose(Point((minx+maxx)/2, (miny+maxy)/2, z), Quaternion()) # Center of the table ps.pose = Pose(Point(minx-DIST_TO_TABLE, (miny+maxy)/2, z), Quaternion()) # Pos for the navigation goal raw_input('Press a key when ready to publish the data.\n') # To wait for publish publisher.publish(ps) return succeeded
def create_probability_route(userdata): furn_prob_map = rospy.get_param(FURN_PROB_PARAM) n = len(userdata.in_nodes) route = range(n) # Get a list of tuples (probability, index_of_name) prob_index = zip(reduce(lambda acc, x: acc + [furn_prob_map[x]], userdata.in_furniture_name_list, []), route) prob_index.sort(key=lambda x: x[0], reverse=True) # Sort by probability robot_pose = Pose() robot_pose.position.x = 0 robot_pose.position.y = 0 robot_pose.position.z = 0 # Get robot's pose in /map coordinates robot_pose = transform_pose(robot_pose, '/base_link', '/map', timeout=3) actual = (robot_pose.position.x, robot_pose.position.y) route[0] = prob_index[0][1] for i in xrange(1, n): route[i] = prob_index[i][1] if prob_index[i][0] == 0: # If probability is 0 we don't look at it. del route[i] del userdata.in_furniture_name_list[i] elif prob_index[i][0] == prob_index[i-1][0]: # If two things have the same probability, get the closer one di_r = ofb_utils.euclidean_distance(userdata.in_nodes[route[i]], actual) di1_r = ofb_utils.euclidean_distance(userdata.in_nodes[route[i-1]], actual) if di_r < di1_r: # The index at i is nearer than the one at i-1 route[i] = route[i-1] route[i-1] = prob_index[i][1] userdata.out_route = route userdata.out_furniture_list = userdata.in_furniture_name_list return succeeded
def detect_faces_cb_ros_pkg(userdata, status, result): '''This code detects faces using the ROS package and then checks the list of people who are not able to walk. If the distance of people who are not able to walk is close to the detected faces they are popped from the list. First perosn who is closer than ---closestPersonDistance--- are sent to userdata''' LOCATION_LIST_DISTANCE = 1 print "CallBack is called" people = result.face_positions # pose_in_map = transform_pose(people.pos, people.header.frame_id, "/map") if userdata.location_list: for person in people: pose_in_stereo = Pose() pose_in_stereo.position = person.pos pose_in_map = transform_pose(pose_in_stereo, userdata.message.header.frame_id, "/map") for not_walking in userdata.location_list: distance_from_list = ((pose_in_map.position.x - not_walking[0])**2 + (pose_in_map.position.pos.y - not_walking[1])**2) if distance_from_list < LOCATION_LIST_DISTANCE: people.pop(people.index(person)) closestPerson = None closestPersonDistance = sys.maxint for person in people: dist = person.pos.z if dist < closestPersonDistance: closestPerson = person closestPersonDistance = dist # FIXME: blacklist already seen people userdata.closest_person = closestPerson print "Closest person (from find_person_with_visit_checker): ", closestPerson return succeeded if closestPerson else aborted
def move_base_goal(): move_base_goal = MoveBaseGoal() move_base_goal.target_pose.header.frame_id = "/map" pose = Pose() pose.position = Point(0, 0, 0) pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 3.14159)) rospy.loginfo(sm.c.BACKGROUND_YELLOW + "Position in /base_link:\n%s %s" % (str(pose), sm.c.NATIVE_COLOR)) pose = transform_pose(pose, "/base_link", "/map") rospy.loginfo(sm.c.BACKGROUND_GREEN + "Position in /map:\n%s %s" % (str(pose), sm.c.NATIVE_COLOR)) move_base_goal.target_pose.pose = pose rospy.loginfo(sm.c.BACKGROUND_YELLOW + 'Sending robot to pose %s%s' % (str(move_base_goal), sm.c.NATIVE_COLOR)) move_base_goal.target_pose.header.stamp = rospy.Time.now() return move_base_goal
def convert_to_base_link(userdata): """ This function convert the received pose from the kinect to base_link """ pose = Pose() pose.position = userdata.in_position.position3D if math.isnan(pose.position.x) or math.isnan(pose.position.y) or math.isnan(pose.position.z): userdata.out_goal_position = userdata.in_position return aborted parent_frame_id = "/gesture_detection_frame" outpose = transform_pose(pose, parent_frame_id, "/base_link") outpose.position.z = 0.0 userdata.out_goal_position = outpose return succeeded
def check_gather_data(userdata): furn_names = userdata.in_furniture_response[0] furn_poses = userdata.in_furniture_response[1] for i in xrange(len(furn_names)): i_pose = (furn_poses[i].pose.position.x, furn_poses[i].pose.position.y) for n, p in userdata.in_furniture_info: p_pose = (p.pose.position.x, p.pose.position.y) if (furn_names[i] == n) and (ofb_utils.euclidean_distance(p_pose, i_pose) <= distance_treshold): break else: # The loop ended without breaking furn_poses[i].pose = transform_pose(furn_poses[i].pose, furn_poses[i].header.frame_id, '/base_link', timeout=3) furn_poses[i].header.frame_id = '/base_link' userdata.in_furniture_info.append((furn_names[i], furn_poses[i])) userdata.out_furniture_info = userdata.in_furniture_info return succeeded
def move_to_caller_goal_cb(userdata, nav_goal): """ Returns a MoveBaseGoal object with the position where the movement was detected and set userdata.out_caller_position variable with a PoseStamped object """ move_base_goal = MoveBaseGoal() move_base_goal.target_pose.header.frame_id = "/map" pose_detected = pose_at_distance(userdata.in_goal_position, 0.5) pose_detected.position.z = 0 teta = math.atan2(pose_detected.position.y, pose_detected.position.x) pose_detected.orientation = Quaternion(*quaternion_from_euler(0, 0, teta)) pose = transform_pose(pose_detected, "/base_link", "/map") move_base_goal.target_pose.pose = pose userdata.out_caller_position = move_base_goal.target_pose move_base_goal.target_pose.header.stamp = Time.now() return move_base_goal
def handle_next_location(self, req): print "Next probable location service: Recieved request for next probable search location in %s" % req.room ret = NextProbableLocationResponse() # Look through all the location with the same probabilities and get the minimum nexts = self._cols[req.room].next_same() min_param = rospy.get_param(PARAM_NAME + req.room + '/' + nexts[0][0]) ret.location = nexts[0][0] if len(nexts) > 1: make_plan = None if self._make_plan: try: rospy.wait_for_service('/move_base/make_plan', 0.1) make_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan) except rospy.ROSException: pass # make_plan is already set to None robot_pose = Pose() robot_pose.position.x = 0 robot_pose.position.y = 0 robot_pose.position.z = 0 robot_pose = transform_pose(robot_pose, '/base_link', '/map', timeout=3) # Get robot's pose in /map coordinates robot_tuple = (robot_pose.position.x, robot_pose.position.y) pose_tuple = (min_param[1], min_param[2]) min_dist = ofb_utils.distance(robot_tuple, pose_tuple, make_plan) for name, _ in nexts[1:]: param = rospy.get_param(PARAM_NAME + req.room + '/' + name) pose_tuple = (param[1], param[2]) d = ofb_utils.distance(robot_tuple, pose_tuple, make_plan) if d < min_dist: min_dist = d min_param = param ret.location = name # All the locations in nexts have the same probabilty self._cols[req.room].swap_elements(nexts[0], (ret.location, nexts[0][1])) pose = Pose() pose.position = Point(min_param[1], min_param[2], 0) pose.orientation = Quaternion(*quaternion_from_euler(0, 0, min_param[3])) ret.loc_position = pose return ret
def check_ttop_data(userdata): table_data = userdata.in_tabletop_data for t in table_data: z = t.pose.pose.position.z q = Quaternion(*quaternion_from_euler(0, 0, 0)) table_pose = Pose(Point(t.x_min-dist_to_table, (t.y_min+t.y_max)/2, z), q) src_frame = t.pose.header.frame_id # src_frame should be either base_link or base_footprint table_pose = transform_pose(table_pose, src_frame, DEST_FRAME, timeout=3) tuple_pose = (table_pose.position.x, table_pose.position.y) for tp in userdata.in_pose_list: if ofb_utils.euclidean_distance(tp, tuple_pose) <= distance_treshold: break else: # The loop ended without breaking userdata.in_pose_list.append(tuple_pose) userdata.out_pose_list = userdata.in_pose_list userdata.in_orientation_list.append(table_pose.orientation) # FIXME table_pose? Should be more_less equal... userdata.out_orientation_list = userdata.in_orientation_list return succeeded
def move_to_caller_goal_cb(userdata, nav_goal): move_base_goal = MoveBaseGoal() move_base_goal.target_pose.header.frame_id = "/map" # move_base_goal.target_pose.header.stamp = rospy.Time.now() if self.referee_position is None: pose_detected = Pose() pose_detected.position=Point(0, 0, 0) pose = transform_pose(pose_detected, "/base_link", "/map") move_base_goal.target_pose.pose = pose # userdata.out_referee_position = move_base_goal self.referee_position = move_base_goal print '************************************\n***********************************************\n*********** Referee Pose Stored ******************\n***************************************************\n*************************************************' else: move_base_goal = self.referee_position move_base_goal.target_pose.header.stamp = rospy.Time.now() return move_base_goal
def check_furn_data(userdata): furniture_data = userdata.in_furniture_data for f_name, f_pose in furniture_data: q = Quaternion(*quaternion_from_euler(0, 0, 0)) f_pose.pose.position.x -= dist_to_furniture furniture_pose = Pose(f_pose.pose.position, q) src_frame = f_pose.header.frame_id # src_frame should be either base_link or base_footprint furniture_pose = transform_pose(furniture_pose, src_frame, DEST_FRAME, timeout=3) tuple_pose = (furniture_pose.position.x, furniture_pose.position.y) for n, tp in zip(userdata.in_name_list, userdata.in_pose_list): if (f_name == n) and (ofb_utils.euclidean_distance(tp, tuple_pose) <= distance_treshold): break else: # The loop ended without breaking userdata.in_pose_list.append(tuple_pose) userdata.out_pose_list = userdata.in_pose_list userdata.in_orientation_list.append(furniture_pose.orientation) userdata.out_orientation_list = userdata.in_orientation_list userdata.in_name_list.append(f_name) userdata.out_name_list = userdata.in_name_list return succeeded
def execute(self, userdata): nav_goal = PoseStamped() nav_goal.header.stamp = rospy.Time.now() nav_goal.header.frame_id = "/map" #nav.goal.pose.position = Point() nav_goal.pose.position.x = rospy.get_param("/params_learn_and_follow_operator_test/nav_goal_x", 1.5) nav_goal.pose.position.y = 0.0 nav_goal.pose.position.z = 0.0 nav_goal.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0)) nav_goal.pose = transform_pose(nav_goal.pose, 'base_link', 'map') global move_base_topic_goal move_base_topic_goal = rospy.get_param("/params_learn_and_follow_operator_test/move_base_topic", "/move_base_simple/goal") self.pub = rospy.Publisher(move_base_topic_goal, PoseStamped) rospy.loginfo('Pose data that will be sent to the topic ' + move_base_topic_goal + ':\n%s' % str(nav_goal)) self.pub.publish(nav_goal) marker = Marker() marker.header.frame_id = "/map" marker.ns = "test_markers_namespace" global marker_id marker.id = marker_id marker_id = marker_id + 1 marker.type = marker.ARROW marker.action = marker.ADD marker.pose = nav_goal.pose marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 0.5 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.5 marker.color.b = 0.1 marker.lifetime = rospy.Duration.from_sec(MARKERS_TAIL_LENGTH * SLEEP_TIME_AFTER_SENT_GOAL) marker.header.stamp = rospy.Time.now() self.marker_pub.publish(marker) person_marker_array = MarkerArray() #HEAD OF THE PERSON BODY person_head_marker = Marker() person_head_marker.header.frame_id = "/map" person_head_marker.ns = "person_test_markers_namespace" person_head_marker.id = 0 person_head_marker.type = marker.SPHERE person_head_marker.action = marker.ADD #we must copy the value to avoid a reference person_head_marker.pose = deepcopy(nav_goal.pose) person_head_marker.pose.position.z = PERSON_HEIGHT - PERSON_HEAD_DIAMETER / 2.0 person_head_marker.scale.x = PERSON_HEAD_DIAMETER person_head_marker.scale.y = PERSON_HEAD_DIAMETER person_head_marker.scale.z = PERSON_HEAD_DIAMETER person_head_marker.color = PERSON_COLOR person_head_marker.lifetime = rospy.Duration.from_sec(SLEEP_TIME_AFTER_SENT_GOAL) person_head_marker.header.stamp = rospy.Time.now() person_marker_array.markers.append(person_head_marker) #TORSO OF THE PERSON BODY person_torso_marker = Marker() person_torso_marker.header.frame_id = "/map" person_torso_marker.ns = "person_test_markers_namespace" person_torso_marker.id = 1 person_torso_marker.type = marker.CUBE person_torso_marker.action = marker.ADD person_torso_marker.pose = deepcopy(nav_goal.pose) person_torso_marker.pose.position.z = PERSON_HEIGHT - PERSON_HEAD_DIAMETER - PERSON_TORSO_HEIGHT / 2.0 person_torso_marker.scale.x = PERSON_TORSO_DIAMETER person_torso_marker.scale.y = PERSON_TORSO_DIAMETER person_torso_marker.scale.z = PERSON_TORSO_HEIGHT person_torso_marker.color = PERSON_COLOR person_torso_marker.lifetime = rospy.Duration.from_sec(SLEEP_TIME_AFTER_SENT_GOAL) person_torso_marker.header.stamp = rospy.Time.now() person_marker_array.markers.append(person_torso_marker) #LEGS OF THE PERSON BODY person_leg_marker = Marker() person_leg_marker.header.frame_id = "/map" person_leg_marker.ns = "person_test_markers_namespace" person_leg_marker.id = 2 person_leg_marker.type = marker.CYLINDER person_leg_marker.action = marker.ADD person_leg_marker.pose.position.x = rospy.get_param("/params_learn_and_follow_operator_test/nav_goal_x", 1.5) person_leg_marker.pose.position.y = PERSON_TORSO_DIAMETER / 4.0 # mid point between edge and center of the body person_leg_marker.pose.position.z = PERSON_HEIGHT - PERSON_HEAD_DIAMETER - PERSON_TORSO_HEIGHT - PERSON_LEG_HEIGHT / 2.0 person_leg_marker.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0)) person_leg_marker.pose = transform_pose(person_leg_marker.pose, 'base_link', 'map') person_leg_marker.scale.x = PERSON_LEG_DIAMETER person_leg_marker.scale.y = PERSON_LEG_DIAMETER person_leg_marker.scale.z = PERSON_LEG_HEIGHT person_leg_marker.color = PERSON_COLOR person_leg_marker.lifetime = rospy.Duration.from_sec(SLEEP_TIME_AFTER_SENT_GOAL) person_leg_marker.header.stamp = rospy.Time.now() person_marker_array.markers.append(person_leg_marker) person_leg_marker = deepcopy(person_leg_marker) person_leg_marker.id = 3 person_leg_marker.pose.position.x = rospy.get_param("/params_learn_and_follow_operator_test/nav_goal_x", 1.5) person_leg_marker.pose.position.y = -PERSON_TORSO_DIAMETER / 4.0 # mid point between edge and center of the body person_leg_marker.pose.position.z = PERSON_HEIGHT - PERSON_HEAD_DIAMETER - PERSON_TORSO_HEIGHT - PERSON_LEG_HEIGHT / 2.0 person_leg_marker.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0)) person_leg_marker.pose = transform_pose(person_leg_marker.pose, 'base_link', 'map') person_marker_array.markers.append(person_leg_marker) self.marker_array_pub.publish(person_marker_array) rospy.sleep(SLEEP_TIME_AFTER_SENT_GOAL) return succeeded
def set_caller_position(userdata): out_pose = PoseStamped() out_pose.header.frame_id = "/map" out_pose.pose = transform_pose(Pose(), "/base_link", "/map") userdata.out_caller_position = out_pose return succeeded
def move_enter_room_cb(userdata, nav_goal): pose = Pose() pose.position = Point(distance+self.door_position, 0, 0) pose.orientation = Quaternion(*quaternion_from_euler(0, 0, orient_after_passing)) nav_goal.target_pose.pose = transform_pose(pose, 'base_link', 'map') return nav_goal
def printOdometryBag(userdata): ''' bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag') odometry_marker_pub = rospy.Publisher("/track_operator/odometry_elevator", Marker) i = 0 for topic, msg, t in bag.read_messages(topics=['/base_odometry/odom']): #rospy.loginfo(str(topic) + '\n\n\n') #rospy.loginfo(str(msg) + '\n\n\n') #rospy.loginfo(str(t) + '\n\n\n') marker = Marker() marker.header.frame_id = "/odom" marker.ns = "odometry_elevator_namespace" marker.id = i marker.type = marker.SPHERE marker.action = marker.ADD marker.pose = msg.pose.pose marker.scale.x = 0.5 marker.scale.y = 0.7 marker.scale.z = 0.9 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.lifetime = rospy.Duration(600.0) marker.header.stamp = rospy.Time.now() rospy.loginfo("\033[00;32m" + str(marker) + "\033[m") odometry_marker_pub.publish(marker) i += 1 bag.close() ''' bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag') odometry_marker_pub = rospy.Publisher("/track_operator/baselink_to_odometry_elevator", Marker) i = 0 stack = [] for topic, msg, t in bag.read_messages(topics=['/base_odometry/odom']): #rospy.loginfo(str(topic) + '\n\n\n') #rospy.loginfo(str(msg) + '\n\n\n') #rospy.loginfo(str(t) + '\n\n\n') pose_transformed = transform_pose(msg.pose.pose, '/odom', '/base_link') if (getDebugLevel() >= 3): marker = Marker() marker.header.frame_id = "/base_link" marker.ns = "baselink_to_odometry_elevator_namespace" marker.id = i marker.type = marker.SPHERE marker.action = marker.ADD marker.pose = pose_transformed marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.lifetime = rospy.Duration(600.0) marker.header.stamp = rospy.Time.now() odometry_marker_pub.publish(marker) debugPrint("\033[01;33m" + str(marker) + "\033[m", 3) stack.append(pose_transformed) i += 1 bag.close() #TODO SHOULD WORK MOST OF THE TIMES, BUT IF THE BAG IS EMPTY OR WHITH LESS THAN 2 POSES THIS CODE WILL CRASH!!! try: initial_pose = stack.pop() final_pose = stack.pop() last_dist = euclidean_distance(initial_pose, final_pose) while (len(stack) > 0 and last_dist < 2.0): new_final_pose = stack.pop() new_dist = euclidean_distance(new_final_pose, initial_pose) if (new_dist > last_dist): last_dist = new_dist final_pose = new_final_pose except Exception as ex: debugPrint(str(ex), 0) debugPrint("The bag was empty. Setting a final pose of 2 meters backwards...", 0) initial_pose = Pose() initial_pose.position.x = 0.0 initial_pose.position.y = 0.0 final_pose = Pose() final_pose.position.x = -2.0 final_pose.position.y = 0.0 marker = Marker() marker.header.frame_id = "/base_link" marker.ns = "baselink_to_odometry_elevator_namespace" marker.id = i marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose = initial_pose marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 2.0 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.lifetime = rospy.Duration(600.0) marker.header.stamp = rospy.Time.now() debugPrint(" Publishing \033[01;32m(GREEN)\033[mmarker of the INITIAL pose outside the elevator...", 3) debugPrint(rospy.loginfo("\033[01;32m" + str(marker) + "\033[m"), 3) odometry_marker_pub.publish(marker) marker = Marker() marker.header.frame_id = "/base_link" marker.ns = "baselink_to_odometry_elevator_namespace" marker.id = i + 1 marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose = final_pose marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 1.0 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.lifetime = rospy.Duration(600.0) marker.header.stamp = rospy.Time.now() debugPrint(" Publishing \033[01;33m(RED)\033[mmarker of the FINAL pose outside the elevator...", 3) debugPrint(" \033[01;33m" + str(marker) + "\033[m", 3) odometry_marker_pub.publish(marker) userdata.final_pose = final_pose return succeeded