Python Header.stamp方法代码示例

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
    def test_4_remote(self):
        '''
        Tests remote kill by publishing hearbeat, stopping and checking alarm is raised, then
        publishing hearbeat again to ensure alarm gets cleared.
        '''
        # publishing msg to network
        pub = rospy.Publisher('/network', Header, queue_size=10)
        rate = rospy.Rate(10)
        t_end = rospy.Time.now() + rospy.Duration(1)
        while rospy.Time.now() < t_end:
            h = Header()
            h.stamp = rospy.Time.now()
            pub.publish(h)
            rate.sleep()

        self.reset_update()
        rospy.sleep(8.5)  # Wait slighly longer then the timeout on killboard
        self.assert_raised()

        self.reset_update()
        t_end = rospy.Time.now() + rospy.Duration(1)
        while rospy.Time.now() < t_end:
            h = Header()
            h.stamp = rospy.Time.now()
            pub.publish(h)
            rate.sleep()
        self.AlarmBroadcaster.clear_alarm()
        self.assert_cleared()

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
def talker():

    rate = rospy.Rate(RATE_ROS_PUBLISHING)
    counter_index = NO_START_FRAME  # 1#21#30#x1
    pcloud = PointCloud2()
    header = Header()
    header.stamp = rospy.Time.now()
    header.frame_id = "velodyne"
    cloud = []
    while not rospy.is_shutdown():

        file_string = "scan" + format(counter_index, "03d")
        cloud_new, cloud_raw = publish_xyz(file_string, counter_index, force_2d=FORCE_2D)
        # cloud = cloud + cloud_new
        cloud = cloud_new
        print file_string
        counter_index = counter_index + 1

        # rate.sleep()
        if not NO_ROS_PUBLISHING:
            pcloud = PointCloud2()
            header = Header()
            header.stamp = rospy.Time.now()
            header.frame_id = "velodyne"  #'odom' #'pose' #'frame' #'velodyne'
            # header.frame_id = 'frame' #'odom' #'pose' #'frame' #'velodyne'
            # header.frame_id = 'odom' #'odom' #'pose' #'frame' #'velodyne'
            pcloud = pc2.create_cloud_xyz32(header, cloud)
            pub_cloud.publish(pcloud)
            header.frame_id = "odom"  #'odom' #'pose' #'frame' #'velodyne'
            pcloud = pc2.create_cloud_xyz32(header, cloud_raw)
            pub_raw.publish(pcloud)

        if counter_index > NO_LAST_FRAME:  # 47:#91:#65:#19 :#65: #65: # 20 :#65 :# 468:
            break
            counter_index = 1
        rate.sleep()

    saveCloud("points.pts", cloud)

    # pcloud = pc2.create_cloud_xyz32(header, cloud)
    # print pcloud.header, pcloud.height, pcloud.width, pcloud.point_step, pcloud.row_step, pcloud.fields
    # pub_cloud.publish(pcloud)

    # rospy.spin()

    # pos = np.random.random(size=(100,3))
    # print cloud
    pos = np.matrix(cloud)
    # pos *= [10,-10,10]
    # pos[0] = (0,0,0)
    # color = np.ones((pos.shape[0], 4))
    # d2 = (pos**2).sum(axis=1)**0.5
    # size = np.random.random(size=pos.shape[0])*10
    size = np.ones((1, pos.shape[0]))
    sp2 = gl.GLScatterPlotItem(pos=pos, color=(1, 1, 1, 1), size=size)
    w.addItem(sp2)

    if (sys.flags.interactive != 1) or not hasattr(QtCore, "PYQT_VERSION"):
        QtGui.QApplication.instance().exec_()

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
 def to_twist_stamped_msg(*args):
     header, homo_mat, _, euler_rot = PoseConverter._make_generic(args)
     if homo_mat is None:
         rospy.logwarn("[pose_converter] Unknown pose type.")
         return None, None, None, None
     twist_stamped = TwistStamped()
     header_msg = Header()
     if header is None:
         header_msg.stamp = rospy.Time.now()
     else:
         header_msg.seq = header[0]
         header_msg.stamp = header[1]
         header_msg.frame_id = header[2]
     return TwistStamped(header_msg, Twist(Vector3(*homo_mat[:3,3].T.A[0]), Vector3(*euler_rot)))

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
def publish_poses_grasps(grasps):
    rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
    grasp_publisher = rospy.Publisher("grasp_pose_from_block_bla", PoseArray)
    graspmsg = Grasp()
    grasp_PA = PoseArray()
    header = Header()
    header.frame_id = "base_link"
    header.stamp = rospy.Time.now()
    grasp_PA.header = header
    for graspmsg in grasps:
        print graspmsg
        print type(graspmsg)
        p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
        grasp_PA.poses.append(p)
    grasp_publisher.publish(grasp_PA)
    rospy.loginfo("Press a to continue...")
    while True:
        choice = raw_input("> ")
    
        if choice == 'a' :
            print "Continuing, a pressed"
            break
        else:
            grasp_publisher.publish(grasp_PA)
            rospy.sleep(0.1)

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
def createRandomGrasps(grasp_pose, num_grasps=1):
    """ Returns a list of num_grasps Grasp's around grasp_pose """
    list_grasps = []
    for grasp_num in range(num_grasps):
        my_pre_grasp_approach = createGripperTranslation(Vector3(1.0, 0.0, 0.0)) # rotate over here?
        my_post_grasp_retreat = createGripperTranslation(Vector3(0.0, 0.0, 1.0))
        header = Header()
        header.frame_id = "base_link"
        header.stamp = rospy.Time.now()
        grasp_pose_copy = copy.deepcopy(grasp_pose)
        modified_grasp_pose = PoseStamped(header, grasp_pose_copy)
        #modified_grasp_pose.pose.position.x -= random.random() * 0.25 # randomize entrance to grasp in 25cm in x
        #modified_grasp_pose.pose.position.y -= random.random() * 0.10 # randomize entrance to grasp in 10cm in y
        #modified_grasp_pose.pose.orientation.z = -1.0
        
        grasp = createGrasp(modified_grasp_pose, # change grasp pose?
                    allowed_touch_objects=["part"], 
                    pre_grasp_posture=getPreGraspPosture(),
                    grasp_posture=getPreGraspPosture(),
                    pre_grasp_approach=my_pre_grasp_approach,
                    post_grasp_retreat=my_post_grasp_retreat,
                    id_grasp="random_grasp_" + str(grasp_num)
                    )
        list_grasps.append(grasp)
    return list_grasps

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
def talker():
  header = Header()
  header.stamp = rospy.Time.now()
  header.frame_id = 'map'
  g_pcloud = pc2.create_cloud_xyz32(header, wmap)
  g_pub_map.publish(g_pcloud)
  rospy.spin()

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
	def __init__(self):
		#Setup subscriber for combined searched image
		frontierMarkerSub = rospy.Subscriber('frontierMarkers',Marker,self.markerCallback,queue_size = 1) 
		
		#Get Number of Robots
		self.numRobots = rospy.get_param('/num_robots')
		
		#Initialize Goal Publishers
		self.goalPub = []
		for i in range(0, self.numRobots):
			#Setup Subscribers to individual robot searched grids
			topicName = 'robot_' + `i` +'/move_base_simple/goal'
			self.goalPub.append(rospy.Publisher(topicName,PoseStamped))
			
		#Setup start/stop time publishers
		self.startPub = rospy.Publisher('startTime', Header)
		self.stopPub = rospy.Publisher('stopTime', Header)
		
		#Get simulation start time
		rospy.sleep(0.01)						#Without this line, get_rostime always returns 0
		self.start_time = rospy.get_rostime()
		rospy.loginfo("SIMULATION STARTED AT: %i %i", self.start_time.secs, self.start_time.nsecs)
		
		#Publish Start TIme
		startMsg = Header()
		startMsg.stamp = self.start_time
		startMsg.frame_id = '0'
		self.startPub.publish(startMsg)
		
		#Wait for subscribed messages to start arriving
		rospy.spin()

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
    def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True):

        header = Header()
        header.frame_id = 'torso_base_link'
        header.stamp = rospy.Time.now()

        moveit_goal = MoveGroupGoal()
        goal_c = Constraints()
        position_c = PositionConstraint()
        position_c.header = header
        if end_link_name != None: 
            position_c.link_name = end_link_name
        position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) 
        position_c.constraint_region.primitive_poses.append(goal_pose)
        position_c.weight = 1.0
        goal_c.position_constraints.append(position_c)
        orientation_c = OrientationConstraint()
        orientation_c.header = header
        if end_link_name != None:
            orientation_c.link_name = end_link_name
        orientation_c.orientation = goal_pose.orientation
        orientation_c.absolute_x_axis_tolerance = 0.01 
        orientation_c.absolute_y_axis_tolerance = 0.01
        orientation_c.absolute_z_axis_tolerance = 0.01
        orientation_c.weight = 1.0
        goal_c.orientation_constraints.append(orientation_c)
        moveit_goal.request.goal_constraints.append(goal_c)
        moveit_goal.request.num_planning_attempts = 5 
        moveit_goal.request.allowed_planning_time = 5.0
        moveit_goal.planning_options.plan_only = plan_only
        moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
        moveit_goal.request.group_name = group
    
        return moveit_goal

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
def fvToMsg(fv, now):

	global seq

	fvMsg = PersonFVMsg()
	
	header = Header()
	header.seq = seq
	header.stamp = now
	header.frame_id = frameID

	rhv = Vector3()
	rev = Vector3()
	lhv = Vector3()
	lev = Vector3()

	setV3(rhv, fv, 0)
	setV3(lhv, fv, 3)
	setV3(rev, fv, 6)
	setV3(lev, fv, 9)

	fvMsg.header = header
	fvMsg.rh = rhv
	fvMsg.re = rev
	fvMsg.lh = lhv
	fvMsg.le = lev

	fvMsg.distance = fv[12]

	return fvMsg

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
    def test_AG_remote(self):
        self.assertEqual(self.AlarmListener.is_cleared(), True,
                         msg='current state of kill signal is raised')
        # publishing msg to network
        pub = rospy.Publisher('/network', Header, queue_size=10)
        rate = rospy.Rate(10)
        t_end = rospy.Time.now() + rospy.Duration(3)
        while rospy.Time.now() < t_end:
            hello_header = Header()
            hello_header.stamp = rospy.Time.now()
            rospy.loginfo(hello_header)
            pub.publish(hello_header)
            rate.sleep()

        self.assertEqual(
            self.AlarmListener.is_cleared(),
            True,
            msg='REMOTE shutdown not worked. State = {}'.format(
                self._current_alarm_state))

        # stop sending the msg, the remote alarm will raise when stop recieving
        # the msg for 8 secs
        rospy.sleep(8.2)

        self.assertEqual(
            self.AlarmListener.is_raised(),
            True,
            msg='REMOTE raised not worked. State = {}'.format(
                self._current_alarm_state))

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
    def execute(self, userdata):
        header = Header()
        header.frame_id = "base_link"
        header.stamp = rospy.Time.now()
        userdata.move_it_joint_goal = MoveGroupGoal()
        goal_c = Constraints()
        for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose):
            joint_c = JointConstraint()
            joint_c.joint_name = name
            joint_c.position = value
            joint_c.tolerance_above = 0.01
            joint_c.tolerance_below = 0.01
            joint_c.weight = 1.0
            goal_c.joint_constraints.append(joint_c)

        userdata.move_it_joint_goal.request.goal_constraints.append(goal_c)
        userdata.move_it_joint_goal.request.num_planning_attempts = 5
        userdata.move_it_joint_goal.request.allowed_planning_time = 5.0
        userdata.move_it_joint_goal.planning_options.plan_only = False  # False = Plan + Execute ; True = Plan only
        userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True
        userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group
        rospy.loginfo("Group Name: " + userdata.manip_joint_group)
        rospy.loginfo("Joints name: " + str(userdata.manip_joint_names))
        rospy.loginfo("Joints Values: " + str(userdata.manip_goal_joint_pose))

        rospy.loginfo("GOAL TO SEND IS:... " + str(userdata.move_it_joint_goal))
        return "succeeded"

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
 def HeaderGenerater(self):
  self.seq += 1
  header = Header()
  header.seq = self.seq
  header.stamp = rospy.Time.now()
  header.frame_id = self.GoalFrame
  return header

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
    def Publish(self):
        imu_data = self._hal_proxy.GetImu()

        imu_msg = Imu()
        h = Header()
        h.stamp = rospy.Time.now()
        h.frame_id = self._frame_id

        imu_msg.header = h
        imu_msg.orientation_covariance = (-1., )*9
        imu_msg.linear_acceleration_covariance = (-1., )*9
        imu_msg.angular_velocity_covariance = (-1., )*9

        imu_msg.orientation.w = imu_data['orientation']['w']
        imu_msg.orientation.x = imu_data['orientation']['x']
        imu_msg.orientation.y = imu_data['orientation']['y']
        imu_msg.orientation.z = imu_data['orientation']['z']

        imu_msg.linear_acceleration.x = imu_data['linear_accel']['x']
        imu_msg.linear_acceleration.y = imu_data['linear_accel']['y']
        imu_msg.linear_acceleration.z = imu_data['linear_accel']['z']

        imu_msg.angular_velocity.x = imu_data['angular_velocity']['x']
        imu_msg.angular_velocity.y = imu_data['angular_velocity']['y']
        imu_msg.angular_velocity.z = imu_data['angular_velocity']['z']

        # Read the x, y, z and heading
        self._publisher.publish(imu_msg)

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
    def cbClusters(self, ros_data):
	#Wait for filename to be received (if receiving from rostopic)
	if self.filename is None:
	  return

 	if self.run is False:
	  self.pubMessages()
	  return

	#Initialize object feature values
        if self.initialized is False:
          print "Reading object features from " + self.filename
	  self.loadObjects()
	  self.initialized = True

	#Read cluster message
	clusterArr = ros_data
        clusters = ros_data.objects
        self.transforms = ros_data.transforms

	#Classify clusters
	#self.labeled, self.tracked, self.errors, self.ids = self.run_filter(self.initX, self.labels, clusters)
	self.tracked, self.ids, self.error = self.getMatchingLabels(self.initX, self.labels, clusters)

	#Publish labels
	if len(clusters) is 0 or self.tracked is None:
	    return
	self.outMsg = LabeledObjects()
	msgTime = rospy.Time.now()
	head = Header()
	head.stamp = msgTime
	self.outMsg.header = head
	self.outMsg.objects = self.tracked
	self.outMsg.labels = self.ids
	self.pubMessages()

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import stamp [as 别名]
def computeICPBetweenScans(no1,no2, init_x = 0 , init_y = 0, init_yaw = 0):
	header = Header()
	header.stamp = rospy.Time.now()
	header.frame_id = 'ibeo'

	example = cython_catkin_example.PyCCExample()

	# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no1)+'.txt')
	# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no1)+'.txt')
	# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no1)+'.txt')
	# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no1)+'.txt')


	# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no1)+'.txt')

	if opts.use_accumulated:
		test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no1)+'.txt'))
		if opts.icp_use_filtered_data:
			test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no1)+'.txt'))
		else:
			test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no1)+'.txt'))

	H_points = [[p[0], p[1],1] for p in test_cloud]
	# H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud]
	pc_point_t1 = pc2.create_cloud_xyz32(header, H_points)
	# print 'text1: ',len(test_cloud)
	# print test_cloud
	# example.load_2d_array('ref_map',np.array(test_cloud, np.float32)) 

	# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no2)+'.txt')
	# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no2)+'.txt')
	# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no2)+'.txt')
	# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no2)+'.txt')


	# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no2)+'.txt')

	if opts.use_accumulated:
		test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no2)+'.txt'))
		if opts.icp_use_filtered_data:
			test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no2)+'.txt'))
		else:
			test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no2)+'.txt'))

	H_points = [[p[0], p[1],1] for p in test_cloud2]
	# H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud]
	pc_point_t2 = pc2.create_cloud_xyz32(header, H_points)
	# print 'text2: ',len(test_cloud)
	# example.load_2d_array('que_map',np.array(test_cloud, np.float32)) 
	# names = example.get_point_xyz_clouds_names()
	# print("Current point clouds: " + ",".join(names))

	# icp_ros_result = g_processICP_srv(pc_point_t1, pc_point_t2)
	# print icp_ros_result


	# cython_icp_result = example.processICP(np.array(test_cloud, np.float32),np.array(test_cloud2, np.float32))
	cython_icp_result = example.processICP(np.array([x[0] for x in test_cloud], np.float32),np.array([x[1] for x in test_cloud], np.float32),np.array([x[0] for x in test_cloud2], np.float32),np.array([x[1] for x in test_cloud2], np.float32), init_x, init_y, init_yaw)
	# print cython_icp_result
	return cython_icp_result