def move(self, goal):
            # Send the goal pose to the MoveBaseAction server
            self.move_base.send_goal(goal)
            
            # Allow 1 minute to get there
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) 
            
            # If we don't get there in time, abort the goal
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.loginfo("Timed out achieving goal")
            else:
                # We made it!
                state = self.move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!") 

def move_to_position(position, orientation):
    """Send a cartesian goal to the action server."""
    global position_client
    if position_client is None:
        init()

    goal = kinova_msgs.msg.ArmPoseGoal()
    goal.pose.header = std_msgs.msg.Header(frame_id=('m1n6s200_link_base'))
    goal.pose.pose.position = geometry_msgs.msg.Point(
        x=position[0], y=position[1], z=position[2])
    goal.pose.pose.orientation = geometry_msgs.msg.Quaternion(
        x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3])

    position_client.send_goal(goal)

    if position_client.wait_for_result(rospy.Duration(10.0)):
        return position_client.get_result()
    else:
        position_client.cancel_all_goals()
        print('        the cartesian action timed-out')
        return None 

def convert_pose(pose, from_frame, to_frame):
    """
    Convert a pose or transform between frames using tf.
        pose            -> A geometry_msgs.msg/Pose that defines the robots position and orientation in a reference_frame
        from_frame      -> A string that defines the original reference_frame of the robot
        to_frame        -> A string that defines the desired reference_frame of the robot to convert to
    """
    global tfBuffer, listener

    # Create Listener objet to recieve and buffer transformations
    trans = None

    try:
        trans = tfBuffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), e:
        print(e)
        rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (to_frame, from_frame))
        return None 

def set_finger_positions(finger_positions):
    """Send a gripper goal to the action server."""
    global gripper_client
    global finger_maxTurn

    finger_positions[0] = min(finger_maxTurn, finger_positions[0])
    finger_positions[1] = min(finger_maxTurn, finger_positions[1])

    goal = kinova_msgs.msg.SetFingersPositionGoal()
    goal.fingers.finger1 = float(finger_positions[0])
    goal.fingers.finger2 = float(finger_positions[1])
    # The MICO arm has only two fingers, but the same action definition is used
    if len(finger_positions) < 3:
        goal.fingers.finger3 = 0.0
    else:
        goal.fingers.finger3 = float(finger_positions[2])
    gripper_client.send_goal(goal)
    if gripper_client.wait_for_result(rospy.Duration(5.0)):
        return gripper_client.get_result()
    else:
        gripper_client.cancel_all_goals()
        rospy.WARN('        the gripper action timed-out')
        return None 

def goto_joint_positions(self, positions_goal):

        positions_cur = self.get_joint_positions()
        assert len(positions_goal) == len(positions_cur)

        duration = norm((r_[positions_goal] - r_[positions_cur]) / self.vel_limits, ord=inf)

        jt = tm.JointTrajectory()
        jt.joint_names = self.joint_names
        jt.header.stamp = rospy.Time.now()

        jtp = tm.JointTrajectoryPoint()
        jtp.positions = positions_goal
        jtp.velocities = zeros(len(positions_goal))
        jtp.time_from_start = rospy.Duration(duration)

        jt.points = [jtp]
        self.controller_pub.publish(jt)

        rospy.loginfo("%s: starting %.2f sec traj", self.controller_name, duration)
        self.pr2.start_thread(JustWaitThread(duration)) 

def follow_timed_joint_trajectory(self, positions, velocities, times):

        jt = tm.JointTrajectory()
        jt.joint_names = self.joint_names
        jt.header.stamp = rospy.Time.now()

        for (position, velocity, time) in zip(positions, velocities, times):
            jtp = tm.JointTrajectoryPoint()
            jtp.positions = position
            jtp.velocities = velocity
            jtp.time_from_start = rospy.Duration(time)
            jt.points.append(jtp)

        self.controller_pub.publish(jt)
        rospy.loginfo("%s: starting %.2f sec traj", self.controller_name, times[-1])
        self.pr2.start_thread(JustWaitThread(times[-1])) 

def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict):
        pnt = JointTrajectoryPoint()
        pnt.time_from_start = rospy.Duration(cmd_time)
        num_joints = b_matrix.shape[0]
        pnt.positions = [0.0] * num_joints
        if dimensions_dict['velocities']:
            pnt.velocities = [0.0] * num_joints
        if dimensions_dict['accelerations']:
            pnt.accelerations = [0.0] * num_joints
        for jnt in range(num_joints):
            b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t)
            # Positions at specified time
            pnt.positions[jnt] = b_point[0]
            # Velocities at specified time
            if dimensions_dict['velocities']:
                pnt.velocities[jnt] = b_point[1]
            # Accelerations at specified time
            if dimensions_dict['accelerations']:
                pnt.accelerations[jnt] = b_point[-1]
        return pnt 

def _get_minjerk_point(self, m_matrix, idx, t, cmd_time, dimensions_dict):
        pnt = JointTrajectoryPoint()
        pnt.time_from_start = rospy.Duration(cmd_time)
        num_joints = m_matrix.shape[0]
        pnt.positions = [0.0] * num_joints
        if dimensions_dict['velocities']:
            pnt.velocities = [0.0] * num_joints
        if dimensions_dict['accelerations']:
            pnt.accelerations = [0.0] * num_joints
        for jnt in range(num_joints):
            m_point = minjerk.minjerk_point(m_matrix[jnt, :, :, :], idx, t)
            # Positions at specified time
            pnt.positions[jnt] = m_point[0]
            # Velocities at specified time
            if dimensions_dict['velocities']:
                pnt.velocities[jnt] = m_point[1]
            # Accelerations at specified time
            if dimensions_dict['accelerations']:
                pnt.accelerations[jnt] = m_point[-1]
        return pnt 

def revalidate(self, timeout, invalidate_state=True, invalidate_config=True):
        """
        invalidate the state and config topics, then wait up to timeout
        seconds for them to become valid again.
        return true if both the state and config topic data are valid
        """
        if invalidate_state:
            self.invalidate_state()
        if invalidate_config:
            self.invalidate_config()
        timeout_time = rospy.Time.now() + rospy.Duration(timeout)
        while not self.is_state_valid() and not rospy.is_shutdown():
            rospy.sleep(0.1)
            if timeout_time < rospy.Time.now():
                rospy.logwarn("Timed out waiting for node interface valid...")
                return False
        return True 

def __init__(self, limb="right"):
        """
        @param limb: Limb to run CalibrateArm on arm side.
        """
        self._limb=limb
        self._client = actionlib.SimpleActionClient('/calibration_command',
                                   CalibrationCommandAction)
        # Waits until the action server has started up and started
        # listening for goals.
        server_up = self._client.wait_for_server(rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Calibration"
                         " Server to connect. Check your ROS networking"
                         "  and/or reboot the robot.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1) 

def _add_point(self, positions, side, time):
        """
        Appends trajectory with new point

        @param positions: joint positions
        @param side: limb to command point
        @param time: time from start for point in seconds
        """
        #creates a point in trajectory with time_from_start and positions
        point = JointTrajectoryPoint()
        point.positions = copy(positions)
        point.time_from_start = rospy.Duration(time)
        if side == self.limb:
            self.goal.trajectory.points.append(point)
        elif self.gripper and side == self.gripper_name:
            self.grip.trajectory.points.append(point) 

def wait(self):
        """
        Waits for and verifies trajectory execution result
        """
        #create a timeout for our trajectory execution
        #total time trajectory expected for trajectory execution plus a buffer
        last_time = self.goal.trajectory.points[-1].time_from_start.to_sec()
        time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5
        timeout = rospy.Duration(self._slow_move_offset +
                                 last_time +
                                 time_buffer)

        finish = self._limb_client.wait_for_result(timeout)
        result = (self._limb_client.get_result().error_code == 0)

        #verify result
        if all([finish, result]):
            return True
        else:
            msg = ("Trajectory action failed or did not finish before "
                   "timeout/interrupt.")
            rospy.logwarn(msg)
            return False 

def __init__(self, limb, joint_names):
        self._joint_names = joint_names
        ns = 'robot/limb/' + limb + '/'
        self._client = actionlib.SimpleActionClient(
            ns + "follow_joint_trajectory",
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear(limb) 

def _LocalToWorld(self,pose):
        """
        Transform a pose from local frame to world frame
        @param pose The 4x4 transformation matrix containing the pose to transform
        @return The 4x4 transformation matrix describing the pose in world frame
        """
        import rospy
        #Get pose w.r.t world frame
        self.listener.waitForTransform(self.world_frame,self.detection_frame,
                                       rospy.Time(),rospy.Duration(10))
        t, r = self.listener.lookupTransform(self.world_frame,self.detection_frame,
                                             rospy.Time(0))
        
        #Get relative transform between frames
        offset_to_world = numpy.matrix(transformations.quaternion_matrix(r))
        offset_to_world[0,3] = t[0]
        offset_to_world[1,3] = t[1]
        offset_to_world[2,3] = t[2]
        
        #Compose with pose to get pose in world frame
        result = numpy.array(numpy.dot(offset_to_world, pose))
        
        return result 

def _LocalToWorld(self,pose):
        """
        Transform a pose from local frame to world frame
        @param pose The 4x4 transformation matrix containing the pose to transform
        @return The 4x4 transformation matrix describing the pose in world frame
        """
        #Get pose w.r.t world frame
        self.listener.waitForTransform(self.world_frame,self.detection_frame,
                                       rospy.Time(),rospy.Duration(10))
        t, r = self.listener.lookupTransform(self.world_frame,self.detection_frame,
                                             rospy.Time(0))
        
        #Get relative transform between frames
        offset_to_world = numpy.matrix(transformations.quaternion_matrix(r))
        offset_to_world[0,3] = t[0]
        offset_to_world[1,3] = t[1]
        offset_to_world[2,3] = t[2]
        
        #Compose with pose to get pose in world frame
        result = numpy.array(numpy.dot(offset_to_world, pose))
        
        return result 

def convert_pose(pose, from_frame, to_frame):
    """
    Convert a pose or transform between frames using tf.
        pose            -> A geometry_msgs.msg/Pose that defines the robots position and orientation in a reference_frame
        from_frame      -> A string that defines the original reference_frame of the robot
        to_frame        -> A string that defines the desired reference_frame of the robot to convert to
    """
    global tfBuffer, listener

    if tfBuffer is None or listener is None:
        _init_tf()

    try:
        trans = tfBuffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), e:
        print(e)
        rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (to_frame, from_frame))
        return None 

def servo_xy(self):
        print "translating in XY at speed:", self.inc
        d = self.centroid - self.goal_pos

        self.tf_listener.waitForTransform('/'+self.limb+'_hand_camera', '/base', rospy.Time(), rospy.Duration(4.0))
        (trans, rot) = self.tf_listener.lookupTransform('/'+self.limb+'_hand_camera', '/base', rospy.Time(0))
        R = tf.transformations.quaternion_matrix(rot)[:3, :3]
        d = numpy.concatenate( (d, numpy.zeros(1)) )
        d_rot = numpy.dot(R, d) 
        direction = self.inc*d_rot / numpy.linalg.norm(d_rot)
        if not self.outOfRange():
            direction[2] = self.inc
        else:
            direction[2] = 0
        
        direction *= self.inc/ numpy.linalg.norm(direction)
        #print direction

        self.command_ik(direction) 

def solve_goal_point(self, centroid):
        # Find the centroid in the point cloud
        x = int(centroid[0])
        y = int(centroid[1])
        depth = self.get_depth(x, y)
        print depth
        # Get pixel points in camera units
        v = self.camera_model.projectPixelTo3dRay((x, y))
        d_cam = numpy.concatenate( (depth*numpy.array(v), numpy.ones(1))).reshape((4, 1))

        # TODO: is this the right frame transform?
        self.tf_listener.waitForTransform('/base', '/camera_depth_optical_frame', rospy.Time(), rospy.Duration(4))
        (trans, rot) = self.tf_listener.lookupTransform('/base', '/camera_depth_optical_frame', rospy.Time())
        
        camera_to_base = tf.transformations.compose_matrix(translate=trans, angles=tf.transformations.euler_from_quaternion(rot))

        d_base = numpy.dot(camera_to_base, d_cam)
        d_base[2] -= params['object_height']/2.0
        return d_base[0:3] 

def __init__(self, limb):
        self.waiting = False
        self.jointnames = ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']
        ns = 'robot/limb/' + limb + '/'
        self._client = actionlib.SimpleActionClient(
            ns + "follow_joint_trajectory",
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear(limb) 

def solve_goal_pose(self, centroid):
        # Project centroid into 3D coordinates
        center = (centroid[0] - self.camera_x/2, centroid[1] - self.camera_y/2) 
        vec = numpy.array( self.camera_model.projectPixelTo3dRay(center) )
        # Scale it by the IR reading
        d_cam = ( self.ir_reading - self.min_ir_depth - self.object_height ) * vec
        d_cam = numpy.concatenate((d_cam, numpy.ones(1)))
        #print "Camera vector:", d_cam

        # Now transform into the world frame
        self.tf_listener.waitForTransform('/base', '/'+self.limb+'_hand_camera', rospy.Time(), rospy.Duration(4))
        (trans, rot) = self.tf_listener.lookupTransform('/base', '/'+self.limb+'_hand_camera', rospy.Time())
        
        camera_to_base = tf.transformations.compose_matrix(translate=trans, angles=tf.transformations.euler_from_quaternion(rot))

        d_base = numpy.dot(camera_to_base, d_cam)
        return d_base[0:3] 

def verify_update_rate(update_time_remaining, update_rate=10, minimum_update_rate_fraction_allowed=0.1, info=''):
    """
    make sure at least 10% of time is remaining when updates are performed.
    we are converting to nanoseconds here since
    that is the unit in which all reasonable
    rates are expected to be measured
    """
    update_duration_sec = 1.0 / update_rate
    minimum_allowed_remaining_time = update_duration_sec * minimum_update_rate_fraction_allowed
    min_remaining_duration = rospy.Duration(minimum_allowed_remaining_time)
    if update_time_remaining < min_remaining_duration:
        rospy.logwarn_throttle(1.0, 'Not maintaining requested update rate, there may be gaps in the data log!\n'
                               '    Update rate is: ' + str(update_rate) + 'Hz, Duration is '+ str(update_duration_sec) +' sec\n' +
                               '    Minimum time allowed time remaining is: ' + str(minimum_allowed_remaining_time) + ' sec\n'  +
                               '    Actual remaining on this update was: ' + str(float(str(update_time_remaining))/1.0e9) + ' sec\n' +
                               '    ' + info) 

def verify_update_rate(update_time_remaining, update_rate=10, minimum_update_rate_fraction_allowed=0.1, info=''):
    """
    make sure at least 10% of time is remaining when updates are performed.
    we are converting to nanoseconds here since
    that is the unit in which all reasonable
    rates are expected to be measured
    """
    update_duration_sec = 1.0 / update_rate
    minimum_allowed_remaining_time = update_duration_sec * minimum_update_rate_fraction_allowed
    min_remaining_duration = rospy.Duration(minimum_allowed_remaining_time)
    if update_time_remaining < min_remaining_duration:
        rospy.logwarn_throttle(1.0, 'Not maintaining requested update rate, there may be problems with the goal pose predictions!\n'
                               '    Update rate is: ' + str(update_rate) + 'Hz, Duration is ' + str(update_duration_sec) + ' sec\n' +
                               '    Minimum time allowed time remaining is: ' + str(minimum_allowed_remaining_time) + ' sec\n' +
                               '    Actual remaining on this update was: ' + str(float(str(update_time_remaining))/1.0e9) + ' sec\n' +
                               '    ' + info) 

def __init__(self):
        # ROS initialization:
        rospy.init_node('pose_manager')

        self.poseLibrary = dict()
        self.readInPoses()
        self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
                                                       execute_cb=self.executeBodyPose,
                                                       auto_start=False)
        self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
        if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
            try:
                rospy.wait_for_service("stop_walk_srv", timeout=2.0)
                self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
            except:
                rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
                          +"This is normal if there is no nao_walker running.")
            self.stopWalkSrv = None
            self.poseServer.start()

            rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());

        else:
            rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
            rospy.signal_shutdown("Required component missing"); 

def parseXapPoses(self, xaplibrary):
        try:
            poses = xapparser.getpostures(xaplibrary)
        except RuntimeError as re:
            rospy.logwarn("Error while parsing the XAP file: %s" % str(re))
            return

        for name, pose in poses.items():

            trajectory = JointTrajectory()

            trajectory.joint_names = pose.keys()
            joint_values = pose.values()

            point = JointTrajectoryPoint()
            point.time_from_start = Duration(2.0) # hardcoded duration!
            point.positions = pose.values()
            trajectory.points = [point]

            self.poseLibrary[name] = trajectory 

def place_block(self, block):
        self.group.set_num_planning_attempts(10)
        self.group.set_planning_time(3)

        self.update_environment_obstacles()

        place_location = moveit_msgs.msg.PlaceLocation()
        place_location.place_pose.header.frame_id = "world"
        place_location.place_pose.pose = block.place_pose

        place_location.pre_place_approach.direction.header.frame_id = "world"
        place_location.pre_place_approach.direction.vector.z = -1.0
        place_location.pre_place_approach.min_distance = 0.08
        place_location.pre_place_approach.desired_distance = 0.1


        place_location.post_place_retreat.direction.header.frame_id = "world"
        place_location.post_place_retreat.direction.vector.z = 1.0
        place_location.post_place_retreat.min_distance = 0.08
        place_location.post_place_retreat.desired_distance = 0.1

        # open
        place_location.post_place_posture.joint_names = ["right_gripper_l_finger_joint", "right_gripper_r_finger_joint"]
        trajpoint = trajectory_msgs.msg.JointTrajectoryPoint()
        trajpoint.positions = [0.04, 0.04]
        trajpoint.time_from_start = rospy.Duration(0.5)
        place_location.post_place_posture.points = [trajpoint]
        self.group.set_support_surface_name("table2")
        rospy.sleep(1.0)
        return self.group.place("block" + str(self.registered_blocks.index(block)), place_location) 

def init_markers(self):
        # Set up our waypoint markers
        marker_scale = 0.2
        marker_lifetime = 0 # 0 is forever
        marker_ns = 'waypoints'
        marker_id = 0
        marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
        
        # Define a marker publisher.
        self.marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=5)
        
        # Initialize the marker points list.
        self.markers = Marker()
        self.markers.ns = marker_ns
        self.markers.id = marker_id
        self.markers.type = Marker.CUBE_LIST
        self.markers.action = Marker.ADD
        self.markers.lifetime = rospy.Duration(marker_lifetime)
        self.markers.scale.x = marker_scale
        self.markers.scale.y = marker_scale
        self.markers.color.r = marker_color['r']
        self.markers.color.g = marker_color['g']
        self.markers.color.b = marker_color['b']
        self.markers.color.a = marker_color['a']
        
        self.markers.header.frame_id = 'odom'
        self.markers.header.stamp = rospy.Time.now()
        self.markers.points = list() 

def send_urdf(parent_link, root_joint, urdf_filename, duration):
    """
    Send the URDF Fragment located at the specified path.

    @param parent_link: parent link to attach the URDF fragment to
                        (usually <side>_hand)
    @param root_joint: root link of the URDF fragment (usually <side>_gripper_base)
    @param urdf_filename: path to the urdf XML file to load into xacro and send
    @param duration: duration to repeat sending the URDF to ensure it is received
    """
    msg = URDFConfiguration()
    # The updating the time parameter tells
    # the robot that this is a new configuration.
    # Only update the time when an updated internal
    # model is required. Do not continuously update
    # the time parameter.
    msg.time = rospy.Time.now()
    # link to attach this urdf to onboard the robot
    msg.link = parent_link
    # root linkage in your URDF Fragment
    msg.joint = root_joint
    msg.urdf = xacro_parse(urdf_filename)
    pub = rospy.Publisher('/robot/urdf', URDFConfiguration, queue_size=10)
    rate = rospy.Rate(5) # 5hz
    start = rospy.Time.now()
    rospy.loginfo('publishing urdf')
    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()
        if (rospy.Time.now() - msg.time) > rospy.Duration(duration):
            break 

def main():
    """RSDK URDF Fragment Example:
    This example shows a proof of concept for
    adding your URDF fragment to the robot's
    onboard URDF (which is currently in use).
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-f', '--file', metavar='PATH', required=True,
        help='Path to URDF file to send'
    )
    required.add_argument(
        '-l', '--link', required=False, default="right_hand", #parent
        help='URDF Link already to attach fragment to (usually <left/right>_hand)'
    )
    required.add_argument(
        '-j', '--joint', required=False, default="right_gripper_base",
        help='Root joint for fragment (usually <left/right>_gripper_base)'
    )
    parser.add_argument("-d", "--duration", type=lambda t:abs(float(t)),
            default=5.0, help="[in seconds] Duration to publish fragment")
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('rsdk_configure_urdf', anonymous=True)

    if not os.access(args.file, os.R_OK):
        rospy.logerr("Cannot read file at '%s'" % (args.file,))
        return 1
    send_urdf(args.link, args.joint, args.file, args.duration)
    return 0 

def ros_pub_marker(loc,color = "green"):
    val = 0.1
    if (color == "green"):
        val = 0.9

    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    marker.header.frame_id = "/zoe/base_link"
    # marker.header.frame_id = "/zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD

    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    if (len(loc) > 4):
        marker.scale.x = loc[5]
        marker.scale.y = loc[6]
        marker.scale.z = loc[4]

    marker.color.a = 0.4
    marker.color.r = 1 - val
    marker.color.g = val
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    # marker.lifetime = rospy.Duration(0.3)
    marker_pub.publish(marker)
    # rospy.sleep(0.008)
    # # while (True):
    # for i in range(2):
    #     marker_pub.publish(marker)
    # # rospy.sleep(0.1) 

def rosMarker(loc,idx,color = "green",dur=0.2):
    val = 0.1
    if (color == "green"):
        val = 0.9

    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    # marker.header.frame_id = "map"
    marker.header.frame_id = "/zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD

    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    if (len(loc) > 4):
        marker.scale.x = loc[5]
        marker.scale.y = loc[6]
        marker.scale.z = loc[4]

    marker.color.a = 0.4
    marker.color.r = 1 - val
    marker.color.g = val
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    marker.lifetime = rospy.Duration(dur)
    marker.id = idx
    return marker
    # rospy.sleep(0.008)
    # # while (True):
    # for i in range(2):
    #     marker_pub.publish(marker)
    # # rospy.sleep(0.1)