def __pub_initial_position(self, x, y, theta):
        """
        Publishing new initial position (x, y, theta) --> for localization
        :param x x-position of the robot
        :param y y-position of the robot
        :param theta theta-position of the robot
        """
        initpose = PoseWithCovarianceStamped()
        initpose.header.stamp = rospy.get_rostime()
        initpose.header.frame_id = "map"
        initpose.pose.pose.position.x = x
        initpose.pose.pose.position.y = y
        quaternion = self.__yaw_to_quat(theta)

        initpose.pose.pose.orientation.w = quaternion[0]
        initpose.pose.pose.orientation.x = quaternion[1]
        initpose.pose.pose.orientation.y = quaternion[2]
        initpose.pose.pose.orientation.z = quaternion[3]
        self.__initialpose_pub.publish(initpose)
        return 

def __publish_goal(self, x, y, theta):
        """
        Publishing goal (x, y, theta)
        :param x x-position of the goal
        :param y y-position of the goal
        :param theta theta-position of the goal
        """
        self.__old_path_stamp = self.__path.header.stamp
        goal = PoseStamped()
        goal.header.stamp = rospy.get_rostime()
        goal.header.frame_id = "map"
        goal.pose.position.x = x
        goal.pose.position.y = y
        quaternion = self.__yaw_to_quat(theta)
        goal.pose.orientation.w = quaternion[0]
        goal.pose.orientation.x = quaternion[1]
        goal.pose.orientation.y = quaternion[2]
        goal.pose.orientation.z = quaternion[3]
        self.__goal_pub_.publish(goal)
        return 

def count(self):
        #curr_rostime = rospy.get_rostime()
        ## time reset
        #if curr_rostime.is_zero():
        #    if len(self.times) > 0:
        #        # print("time has reset, resetting counters")
        #        self.times = []
        #    return
        curr = time()
        if self.msg_t0 < 0 or self.msg_t0 > curr:
            self.msg_t0 = curr
            self.msg_tn = curr
            self.times = []
        else:
            self.times.append(curr - self.msg_tn)
            self.msg_tn = curr

        #only keep statistics for the last X messages so as not to run out of memory
        if len(self.times) > self.window_size - 1:
            self.times.pop(0) 

def _to_time_inst(msg, rostype, inst=None):
    # Create an instance if we haven't been provided with one
    if rostype == "time" and msg == "now":
        return rospy.get_rostime()

    if inst is None:
        if rostype == "time":
            inst = rospy.rostime.Time()
        elif rostype == "duration":
            inst = rospy.rostime.Duration()
        else:
            return None

    # Copy across the fields
    for field in ["secs", "nsecs"]:
        try:
            if field in msg:
                setattr(inst, field, msg[field])
        except TypeError:
            continue

    return inst 

def getMarkerWindow(x,y,z,r,p,yaw):

	myMarker = Marker()
	myMarker.header.frame_id = "world"
	myMarker.header.seq = 1
	myMarker.header.stamp    = rospy.get_rostime()
	myMarker.ns = "window"
	myMarker.id = 1
	myMarker.type = myMarker.MESH_RESOURCE # sphere
	myMarker.action = myMarker.ADD
	myMarker.pose.position.x = x
	myMarker.pose.position.y = y
	myMarker.pose.position.z = z
	q = quaternion_from_euler(r, p, yaw)
	myMarker.pose.orientation.x=q[0]
	myMarker.pose.orientation.y=q[1]
	myMarker.pose.orientation.z=q[2]
	myMarker.pose.orientation.w=q[3]
	myMarker.mesh_resource = "package://project/models/window_buena.stl";
	myMarker.color=ColorRGBA(0, 1, 0, 1)
	myMarker.scale.x = 5;
	myMarker.scale.y = 5;
	myMarker.scale.z = 6;

	return myMarker 

def update_current_values(self):
        """
        Function to update vehicle control current values.

        we calculate the acceleration on ourselves, because we are interested only in
        the acceleration in respect to the driving direction
        In addition a small average filter is applied

        :return:
        """
        current_time_sec = rospy.get_rostime().to_sec()
        delta_time = current_time_sec - self.info.current.time_sec
        current_speed = self.vehicle_status.velocity
        if delta_time > 0:
            delta_speed = current_speed - self.info.current.speed
            current_accel = delta_speed / delta_time
            # average filter
            self.info.current.accel = (self.info.current.accel * 4 + current_accel) / 5
        self.info.current.time_sec = current_time_sec
        self.info.current.speed = current_speed
        self.info.current.speed_abs = abs(current_speed) 

def _goto_mode_and_indicate(self,requested):
        """
        define the commands for the function
        """
        config_cmd = ConfigCmd()

        """
        Send the mode command
        """
        r = rospy.Rate(10)
        start_time = rospy.get_time()
        while ((rospy.get_time() - start_time) < 30.0) and (MOVO_MODES_DICT[requested] != self.movo_operational_state):
            config_cmd.header.stamp = rospy.get_rostime()
            config_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
            config_cmd.gp_param = requested
            self.cmd_config_cmd_pub.publish(config_cmd)
            r.sleep()

        if (MOVO_MODES_DICT[requested] != self.movo_operational_state):
            rospy.logerr("Could not set operational Mode")
            rospy.loginfo("The platform did not respond, ")
            return False 

def parse(self,data):

        header_stamp = rospy.get_rostime()
        self.init=False
        self._MsgData.header.stamp = header_stamp
        self._MsgData.header.seq = self._seq
        temp = [data[0],data[1],data[2],data[3]]
        self._MsgData.fault_status_words = temp
        self._MsgData.operational_time = convert_u32_to_float(data[4])
        self._MsgData.operational_state = data[5]
        self.op_mode = data[5]
        self._MsgData.dynamic_response = data[6]
        self._MsgData.machine_id = data[8]
        
        self._MsgPub.publish(self._MsgData)
        self._seq += 1
        
        return header_stamp 

def _update_gripper_joint_state(self,dev=0):
        js = JointState()
        js.header.frame_id = ''
        js.header.stamp = rospy.get_rostime()
        js.header.seq = self._seq[dev]
        if 0==dev:
            prefix = 'right_'
        else:
            prefix='left_'
        js.name = ['%srobotiq_85_left_knuckle_joint'%prefix]
        pos = np.clip(0.8 - ((0.8/0.085) * self._gripper.get_pos(dev)), 0., 0.8)
        js.position = [pos]
        dt = rospy.get_time() - self._prev_js_time[dev]
        self._prev_js_time[dev] = rospy.get_time()
        js.velocity = [(pos-self._prev_js_pos[dev])/dt]
        self._prev_js_pos[dev] = pos
        return js 

def GetCurrentRobotPose(self,frame="map"):
        self.tfl.waitForTransform(frame, "base_link", rospy.Time(), rospy.Duration(1.0))
        (trans,rot) = self.tfl.lookupTransform(frame, "base_link", rospy.Time(0))
        
        """
        Remove all the rotation components except yaw
        """
        euler = tf.transformations.euler_from_quaternion(rot)
        rot = tf.transformations.quaternion_from_euler(0,0,euler[2])    
    
        current_pose = PoseWithCovarianceStamped()
        current_pose.header.stamp = rospy.get_rostime()
        current_pose.header.frame_id = frame
        current_pose.pose.pose = Pose(Point(trans[0], trans[1], 0.0), Quaternion(rot[0],rot[1],rot[2],rot[3]))
        
        return current_pose 

def motion_stop(self, duration=1.0):
        self._cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
        self._cfg_cmd.gp_param = 0
        self._cfg_cmd.header.stamp = rospy.get_rostime()
        self._cfg_pub.publish(self._cfg_cmd)

        rospy.logdebug("Stopping velocity command to movo base from BaseVelTest class ...")
        try:
            r = rospy.Rate(10)
            start_time = rospy.get_time()
            while (rospy.get_time() - start_time) < duration:
                self._base_vel_pub.publish(Twist())
                r.sleep()
        except Exception as ex:
            print "Message of base motion failed to be published, error message: ", ex.message
            pass 

def set_header(self, header = None):
        """
        @param header:
          - None: set default header with current timestamp
          - [header]: set message header
        """
        if header is None:
            self._data.header.stamp = rospy.get_rostime()
            self._data.header.seq = 1
            self._data.header.frame_id = "base"
        else:
            self._data.header = header 

def __init__(self):
        rospy.init_node('sound_server', anonymous=False)
        rospy.on_shutdown(self._shutdown)
        while (rospy.get_rostime() == 0.0):
            pass
        rospy.Subscriber("/sound_server/speech_synth", String, self.cb_speech_synth)
        rospy.Subscriber("/sound_server/play_sound_file", String, self.cb_play_sound_file)
        self.pub_status = rospy.Publisher("/sound_server/status", String)
        self.s = mirage_sound_out(festival_cache_path="/home/ubuntu/Music/mirage_engrams/festival_cache/", sound_effects_path="/home/ubuntu/Music/mirage_engrams/sound_effects/")
        rospy.sleep(2.0) # Startup time.
        rospy.loginfo("mirage_sound_out ready")
        rospy.spin() 

def _to_object_inst(msg, rostype, roottype, inst, stack):
    # Typecheck the msg
    if type(msg) is not dict:
        raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))

    # Substitute the correct time if we're an std_msgs/Header
    if rostype in ros_header_types:
        inst.stamp = rospy.get_rostime()

    inst_fields = dict(zip(inst.__slots__, inst._slot_types))

    for field_name in msg:
        # Add this field to the field stack
        field_stack = stack + [field_name]

        # Raise an exception if the msg contains a bad field
        if not field_name in inst_fields:
            raise NonexistentFieldException(roottype, field_stack)

        field_rostype = inst_fields[field_name]
        field_inst = getattr(inst, field_name)

        field_value = _to_inst(msg[field_name], field_rostype,
                    roottype, field_inst, field_stack)

        setattr(inst, field_name, field_value)

    return inst 

def make_ros_request_message(self, qsrlib_request_message):
        """Make a QSRlib ROS service request message from standard QSRlib request message.

        :param qsrlib_request_message: The standard QSRlib request message.
        :type qsrlib_request_message: :class:`QSRlib_Request_Message <qsrlib.qsrlib.QSRlib_Request_Message>`
        :return: The ROS service request message.
        :rtype: qsr_lib.srv.RequestQSRsRequest
        """
        req = RequestQSRsRequest()
        req.header.stamp = rospy.get_rostime()
        req.data = pickle.dumps(qsrlib_request_message)
        return req 

def handle_request_qsrs(self, req):
        """Service handler.

        :param req: QSRlib ROS request.
        :type req: qsr_lib.srv.RequestQSRsRequest
        :return: SRlib ROS response message.
        :rtype: qsr_lib.srv.RequestQSRsResponse
        """
        rospy.logdebug("Handling QSRs request made at %i.%i" % (req.header.stamp.secs, req.header.stamp.nsecs))
        req_msg = pickle.loads(req.data)
        qsrlib_res_msg = self.qsrlib.request_qsrs(req_msg)
        ros_res_msg = RequestQSRsResponse()
        ros_res_msg.header.stamp = rospy.get_rostime()
        ros_res_msg.data = pickle.dumps(qsrlib_res_msg)
        return ros_res_msg 

def handle_multi_range_message(self, multi_range_msg):
        """Handle a ROS multi-range message by updating and publishing the state.

        Args:
             multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message.
        """
        # Update tracker position based on time-of-flight measurements
        new_estimate = self.update_estimate(multi_range_msg)
        if new_estimate is None:
            rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format(
                multi_range_msg.address, multi_range_msg.remote_address))
        else:
            # Publish tracker message
            ros_msg = uwb.msg.UWBTracker()
            ros_msg.header.stamp = rospy.get_rostime()
            ros_msg.address = multi_range_msg.address
            ros_msg.remote_address = multi_range_msg.remote_address
            ros_msg.state = new_estimate.state
            ros_msg.covariance = np.ravel(new_estimate.covariance)
            self.uwb_pub.publish(ros_msg)

            # Publish target transform (rotation is identity)
            self.tf_broadcaster.sendTransform(
                (new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]),
                tf.transformations.quaternion_from_euler(0, 0, 0),
                rospy.get_rostime(),
                self.target_frame,
                self.tracker_frame
            ) 

def draw_rviz(self):
        self.marker.header.stamp.secs = rospy.get_rostime().secs
        self.marker.header.stamp.nsecs = rospy.get_rostime().nsecs
        self.pub.publish(self.marker) 

def execute(self, userdata):
		'''Execute this state'''

		elapsed = rospy.get_rostime() - self._start_time;
		if (elapsed.to_sec() > self._wait):
			return 'done' 

def on_enter(self, userdata):
		'''Upon entering the state, save the current time and start waiting.'''

		self._start_time = rospy.get_rostime()
		
		try:
			self._rate.sleep()
		except ROSInterruptException:
			rospy.logwarn('Skipped sleep.') 

def _wait_for_subscribers(self, pub, timeout=5.0):
        starting_time = rospy.get_rostime()
        rate = rospy.Rate(100)
        while not rospy.is_shutdown():
            elapsed = rospy.get_rostime() - starting_time
            if elapsed.to_sec() >= timeout:
                return False
            if pub.get_num_connections() >= 1:
                return True
            rate.sleep()
        return False 

def update_state(self, state):
        self.state_received = True
        if self.mission is 'trackdrive' or self.mission is 'skidpad':
            cross_line = intersect(self.start_A, self.start_B, to_point(self.last_state), to_point(state))
            if cross_line:
                self.lap_count = self.lap_count + 1
                if self.lap_count == 1:
                    self.starting_time = rospy.get_rostime().to_sec()
                    self.res_go_time = rospy.get_rostime().to_sec()
                else:
                    current_time  = rospy.get_rostime().to_sec()
                    self.lap_time.append(current_time - self.starting_time)
                    self.starting_time = current_time
                    rospy.logwarn("LAP Time: %f", self.lap_time[-1])

                rospy.logwarn("LAP: %i", self.lap_count)
        elif self.mission == 'acceleration':
            cross_line_start = intersect(self.start_A, self.start_B, to_point(self.last_state), to_point(state))
            cross_line_end = intersect(self.end_A, self.end_B, to_point(self.last_state), to_point(state))
            if cross_line_start:
                rospy.logwarn("Starting  to measure")
                self.starting_time = rospy.get_rostime().to_sec()
                self.res_go_time = rospy.get_rostime().to_sec()
            if cross_line_end:
                self.lap_time.append(rospy.get_rostime().to_sec() - self.starting_time)
                rospy.logwarn("STOP  stopwatch wioth time: %f", self.lap_time[-1])

        vel = np.sqrt(state.vx ** 2 + state.vy ** 2)
        if self.vel_avg == 0:
            self.vel_avg = vel
        else:
            self.vel_avg = (vel + self.vel_avg) / 2.0

        self.last_state = state 

def get_duration(self):
        return 0.0 if self.res_go_time == 0.0 else rospy.get_rostime().to_sec() - self.res_go_time 

def _broadcastOdometry(self, odometry_publisher):
        """
        INTERNAL METHOD, computes an odometry message based on the robot's
        position, and broadcast it

        Parameters:
            odometry_publisher - The ROS publisher for the odometry message
        """
        # Send Transform odom
        x, y, theta = self.robot.getPosition()
        odom_trans = TransformStamped()
        odom_trans.header.frame_id = "odom"
        odom_trans.child_frame_id = "base_link"
        odom_trans.header.stamp = rospy.get_rostime()
        odom_trans.transform.translation.x = x
        odom_trans.transform.translation.y = y
        odom_trans.transform.translation.z = 0
        quaternion = pybullet.getQuaternionFromEuler([0, 0, theta])
        odom_trans.transform.rotation.x = quaternion[0]
        odom_trans.transform.rotation.y = quaternion[1]
        odom_trans.transform.rotation.z = quaternion[2]
        odom_trans.transform.rotation.w = quaternion[3]
        self.transform_broadcaster.sendTransform(odom_trans)
        # Set up the odometry
        odom = Odometry()
        odom.header.stamp = rospy.get_rostime()
        odom.header.frame_id = "odom"
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0.0
        odom.pose.pose.orientation = odom_trans.transform.rotation
        odom.child_frame_id = "base_link"
        [vx, vy, vz], [wx, wy, wz] = pybullet.getBaseVelocity(
            self.robot.getRobotModel(),
            self.robot.getPhysicsClientId())
        odom.twist.twist.linear.x = vx
        odom.twist.twist.linear.y = vy
        odom.twist.twist.angular.z = wz
        odometry_publisher.publish(odom) 

def _broadcastJointState(self, joint_state_publisher, extra_joints=None):
        """
        INTERNAL METHOD, publishes the state of the robot's joints into the ROS
        framework

        Parameters:
            joint_state_publisher - The ROS publisher for the JointState
            message, describing the state of the robot's joints
            extra_joints - A dict, describing extra joints to be published. The
            dict should respect the following syntax:
            {"joint_name": joint_value, ...}
        """
        msg_joint_state = JointState()
        msg_joint_state.header = Header()
        msg_joint_state.header.stamp = rospy.get_rostime()
        msg_joint_state.name = list(self.robot.joint_dict)
        msg_joint_state.position = self.robot.getAnglesPosition(
            msg_joint_state.name)

        try:
            assert isinstance(extra_joints, dict)

            for name, value in extra_joints.items():
                msg_joint_state.name += [name]
                msg_joint_state.position += [value]

        except AssertionError:
            pass

        joint_state_publisher.publish(msg_joint_state) 

def _broadcastLasers(self, laser_publisher):
        """
        INTERNAL METHOD, publishes the laser values in the ROS framework

        Parameters:
            laser_publisher - The ROS publisher for the LaserScan message,
            corresponding to the laser info of the pepper robot (for API
            consistency)
        """
        if not self.robot.laser_manager.isActive():
            return

        scan = LaserScan()
        scan.header.stamp = rospy.get_rostime()
        scan.header.frame_id = "base_footprint"
        # -120 degres, 120 degres
        scan.angle_min = -2.0944
        scan.angle_max = 2.0944

        # 240 degres FoV, 61 points (blind zones inc)
        scan.angle_increment = (2 * 2.0944) / (15.0 + 15.0 + 15.0 + 8.0 + 8.0)

        # Detection ranges for the lasers in meters, 0.1 to 3.0 meters
        scan.range_min = 0.1
        scan.range_max = 3.0

        # Fill the lasers information
        right_scan = self.robot.getRightLaserValue()
        front_scan = self.robot.getFrontLaserValue()
        left_scan = self.robot.getLeftLaserValue()

        if isinstance(right_scan, list):
            scan.ranges.extend(list(reversed(right_scan)))
            scan.ranges.extend([-1]*8)
        if isinstance(front_scan, list):
            scan.ranges.extend(list(reversed(front_scan)))
            scan.ranges.extend([-1]*8)
        if isinstance(left_scan, list):
            scan.ranges.extend(list(reversed(left_scan)))

        laser_publisher.publish(scan) 

def update_waypoints(self, waypoints, start_time=None):
        super(RosVehicleControl, self).update_waypoints(waypoints, start_time)
        rospy.loginfo("{}: Waypoints changed.".format(self._role_name))
        path = Path()
        path.header.stamp = rospy.get_rostime()
        path.header.frame_id = "map"
        for wpt in waypoints:
            print(wpt)
            path.poses.append(PoseStamped(pose=trans.carla_transform_to_ros_pose(wpt)))
        self._path_publisher.publish(path) 

def _run_waypoints(self):
        rospy.sleep(5)
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            if ((len(self.waypoints) > 0) and (self.present_waypoint < len(self.waypoints)) and (False == self.waypoint_is_executing) and (True == self.run_waypoints)):
                self.waypoint_is_executing = True
                goal = PoseStamped()
                goal.header.stamp = rospy.get_rostime()
                goal.header.frame_id = self.global_frame
                if ((True == self.waypoints[self.present_waypoint][0]) and (len(self.waypoints)>1)) :
                    pos1 = self.waypoints[self.present_waypoint][1]
                    
                    if (self.present_waypoint == (len(self.waypoints)-1)):
                        pos2 = self.waypoints[0][1]
                    else:
                        pos2 = self.waypoints[self.present_waypoint+1][1]    
                    
                    y2y1= pos2.position.y-pos1.position.y
                    x2x1= pos2.position.x-pos1.position.x
                    heading = tf.transformations.quaternion_from_euler(0,0,atan2(y2y1,x2x1))
                    self.waypoints[self.present_waypoint][1].orientation.x = heading[0]
                    self.waypoints[self.present_waypoint][1].orientation.y = heading[1]
                    self.waypoints[self.present_waypoint][1].orientation.z = heading[2]
                    self.waypoints[self.present_waypoint][1].orientation.w = heading[3]
                    
                goal.pose = self.waypoints[self.present_waypoint][1] 

                self._simple_goal_cb(goal)

            self.marker_array_pub.publish(self.marker_array_msg)
            r.sleep() 

def _preempt_cb(self):
        self.move_base_client.cancel_goals_at_and_before_time(rospy.get_rostime())
        rospy.logwarn("Current move base goal cancelled")
        if (self.move_base_server.is_active()):
            if not self.move_base_server.is_new_goal_available():
                rospy.loginfo("Preempt requested without new goal, cancelling move_base goal.")
                self.move_base_client.cancel_goal()

            self.move_base_server.set_preempted(MoveBaseResult(), "Got preempted by a new goal") 

def _get_current_pose(self):

        """
        Gets the current pose of the base frame in the global frame
        """
        current_pose = None
        listener = tf.TransformListener()
        rospy.sleep(1.0)
        try:
            listener.waitForTransform(self.global_frame, self.base_frame, rospy.Time(), rospy.Duration(1.0))
        except:
            pass
        try:
            (trans,rot) = listener.lookupTransform(self.global_frame, self.base_frame, rospy.Time(0))

            pose_parts = [0.0] * 7
            pose_parts[0]  = trans[0]
            pose_parts[1]  = trans[1]
            pose_parts[2]  = 0.0
            euler = tf.transformations.euler_from_quaternion(rot)
            rot = tf.transformations.quaternion_from_euler(0,0,euler[2])
            pose_parts[3] = rot[0]
            pose_parts[4] = rot[1]
            pose_parts[5] = rot[2]
            pose_parts[6] = rot[3]

            current_pose = PoseWithCovarianceStamped()
            current_pose.header.stamp = rospy.get_rostime()
            current_pose.header.frame_id = self.global_frame
            current_pose.pose.pose = Pose(Point(pose_parts[0], pose_parts[1], pose_parts[2]), Quaternion(pose_parts[3],pose_parts[4],pose_parts[5],pose_parts[6]))
        except:
            rospy.loginfo("Could not get transform from %(1)s->%(2)s"%{"1":self.global_frame,"2":self.base_frame})

        return current_pose