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
Beispiel #10
0
            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
Beispiel #14
0
 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