Python rospy.get_rostime方法代码示例

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_rostime [as 别名]
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