Python Header.frame_id方法代码示例

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import frame_id [as 别名]
    def key_callback(self,msg):
        '''
        function to handle keyboard buttons
        '''
        key = str(msg.data)
        
        #rospy.loginfo("Heard keypress %s", key)
                    
        if key == "a":
            self.keyboard_frame_counter = self.keyboard_frame_counter + 1;

            controlMessage = Header()
            #command to add waypoint;
            controlMessage.seq = 1;
            controlMessage.frame_id = 'Frame_' + str(self.keyboard_frame_counter);
            self._command_pub.publish(controlMessage)

        if key == "d":
            
            controlMessage = Header()
            #command to delete waypoint;
            #deleting last waypoint
            controlMessage.seq = 2;
            controlMessage.frame_id = 'Frame_' + str(self.keyboard_frame_counter);
            self._command_pub.publish(controlMessage)
            if (self.keyboard_frame_counter>0):
                self.keyboard_frame_counter -= 1;
            
        if key == "\x13": #CTRL+ S button code           
            controlMessage = Header()
            #command to save map;
            controlMessage.seq = 3;
            self._command_pub.publish(controlMessage)

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import frame_id [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 frame_id [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 frame_id [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

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import frame_id [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 frame_id [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 frame_id [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 frame_id [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 frame_id [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 frame_id [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 frame_id [as 别名]
    def _cloud_cb(self, cloud):
        points = np.array(list(read_points(cloud, skip_nans=True)))
        if points.shape[0] == 0:
            return

        # Get 4x4 matrix which transforms point cloud co-ords to odometry frame
        try:
            points_to_map = self.tf.asMatrix('/odom', cloud.header)
        except tf.ExtrapolationException:
            return

        transformed_points = points_to_map.dot(np.vstack((points.T, np.ones((1, points.shape[0])))))
        transformed_points = transformed_points[:3,:].T

        if self.fused is None:
            self.fused = transformed_points
        else:
            self.fused = np.vstack((self.fused, transformed_points))

        self.seq += 1
        header = Header()
        header.seq = self.seq
        header.stamp = rospy.Time.now()
        header.frame_id = '/odom'

        output_cloud = create_cloud(header, cloud.fields, self.fused)

        rospy.loginfo('Publishing new cloud')
        self.cloud_pub.publish(output_cloud)

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import frame_id [as 别名]
 def get_ros_header(self):
     header = Header()
     header.stamp = rospy.Time.now()
     header.seq = self.sequence
     # http://www.ros.org/wiki/geometry/CoordinateFrameConventions#Multi_Robot_Support
     header.frame_id = self.frame_id
     return header

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import frame_id [as 别名]
    def _cloud_cb(self, cloud):
        points = np.array(list(read_points(cloud)))
        if points.shape[0] == 0:
            return
        
        pos = points[:,0:3]
        cor = np.reshape(points[:,-1], (points.shape[0], 1))

        # Get 4x4 matrix which transforms point cloud co-ords to odometry frame
        try:
            points_to_map = self.tf.asMatrix('/lasths', cloud.header)
        except tf.ExtrapolationException:
            return

        transformed_points = points_to_map.dot(np.vstack((pos.T, np.ones((1, pos.shape[0])))))
        transformed_points = transformed_points[:3,:].T

        self.seq += 1
        header = Header()
        header.seq = self.seq
        header.stamp = rospy.Time.now()
        header.frame_id = '/lasths'

        self.cloud = np.vstack((self.cloud, np.hstack((transformed_points, cor))))
        if self.seq % 30 == 0:
            print "plup!"
            self.cloud = np.zeros((0, 4))

        output_cloud = create_cloud(header, cloud.fields, self.cloud)
        self.cloud_pub.publish(output_cloud)

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import frame_id [as 别名]
def make_header(frame_id, stamp=None):
    if stamp == None:
        stamp = rospy.Time.now()
    header = Header()
    header.stamp = stamp
    header.frame_id = frame_id
    return header

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

        self.imu_msg.header = h

        q = self.device.read_quaternion()
        self.imu_msg.orientation.x = q[0]
        self.imu_msg.orientation.y = q[1]
        self.imu_msg.orientation.z = q[2]
        self.imu_msg.orientation.w = q[3]

        g = self.device.read_gyroscope()
        # convert from deg/sec to rad/sec
        self.imu_msg.angular_velocity.x = g[0] * math.pi / 180.0
        self.imu_msg.angular_velocity.y = g[1] * math.pi / 180.0
        self.imu_msg.angular_velocity.z = g[2] * math.pi / 180.0

        a = self.device.read_linear_acceleration()
        self.imu_msg.linear_acceleration.x = a[0]
        self.imu_msg.linear_acceleration.y = a[1]
        self.imu_msg.linear_acceleration.z = a[2]

        self.imu_pub.publish(self.imu_msg)

        self.temp_msg.header = h
        self.temp_msg.temperature = self.device.read_temp()
        self.temp_pub.publish(self.temp_msg)