Python std_msgs.msg.Header() Examples

def get_msg_header(self, frame_id=None, timestamp=None):
        """
        Get a filled ROS message header
        :return: ROS message header
        :rtype: std_msgs.msg.Header
        """
        header = Header()
        if frame_id:
            header.frame_id = frame_id
        else:
            header.frame_id = self.get_prefix()
        if timestamp:
            header.stamp = rospy.Time.from_sec(timestamp)
        else:
            header.stamp = self.communication.get_current_ros_time()
        return header 

def navigation():
    pub = rospy.Publisher('usv_position_setpoint', Odometry, queue_size=10) # only create a rostopic, navigation system TODO
    rospy.init_node('navigation_publisher')
    rate = rospy.Rate(60) # 10h

    x = -20.0
    y = -20.0

    msg = Odometry()
   # msg.header = Header()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = "navigation"
    msg.pose.pose.position = Point(x, y, 0.)
 
    while not rospy.is_shutdown():
            pub.publish(msg)
            rate.sleep() 

def __init__(self):
        rospy.init_node('usv_vel_ctrl', anonymous=False)
        self.rate = 10
        r = rospy.Rate(self.rate) # 10hertz

        self.usv_vel = Odometry()
        self.usv_vel_ant = Odometry()
        self.target_vel = Twist()
        self.target_vel_ant = Twist()
        self.thruster_msg = JointState()
        self.rudder_msg = JointState()
        self.thruster_msg.header = Header()
        self.rudder_msg.header = Header()
        self.kp_lin = 80
        self.ki_lin = 200
        thruster_name = 'fwd'
        rudder_name = 'rudder_joint'
        self.I_ant_lin = 0
        self.I_ant_ang = 0
        self.lin_vel = 0
        self.lin_vel_ang = 0
        self.ang_vel = 0
        self.kp_ang = 2 
        self.ki_ang = 4
        self.rudder_max = 70
        self.thruster_max = 30

        self.pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
        self.pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)

        rospy.Subscriber("state", Odometry, self.get_usv_vel)  
        rospy.Subscriber("cmd_vel", Twist, self.get_target_vel)

        while not rospy.is_shutdown():
            self.pub_rudder.publish(self.rudder_ctrl_msg(self.ang_vel_ctrl(), rudder_name))
            self.pub_motor.publish(self.thruster_ctrl_msg(self.lin_vel_ctrl(), thruster_name))
            r.sleep() 

def replicateTransformOverTime(self, transf, orig_frame, dest_frame, frequency, start_time=None, end_time=None):
        """
        Adds a new transform to the graph with the specified value

        This can be useful to add calibration a-posteriori.

        :param transf: value of the transform
        :param orig_frame: the source tf frame of the transform of interest
        :param dest_frame: the target tf frame of the transform of interest
        :param frequency: frequency at which the transform should be published
        :param start_time: the time the transform should be published from
        :param end_time: the time the transform should be published until
        :return:
        """
        if start_time is None:
            start_time = self.getStartTime()
        if end_time is None:
            end_time = self.getEndTime()
        transl, quat = transf
        time_delta = rospy.Duration(1 / frequency)

        t_msg = TransformStamped(header=Header(frame_id=orig_frame),
                                 child_frame_id=dest_frame,
                                 transform=Transform(translation=Vector3(*transl), rotation=Quaternion(*quat)))

        def createMsg(time_nsec):
            time = rospy.Time(time_nsec / 1000000000)
            t_msg2 = copy.deepcopy(t_msg)
            t_msg2.header.stamp = time
            return t_msg2

        new_msgs = [createMsg(t) for t in range(start_time.to_nsec(), end_time.to_nsec(), time_delta.to_nsec())]
        self.tf_messages += new_msgs
        self.tf_messages.sort(key=lambda tfm: tfm.header.stamp.to_nsec())
        self.tf_times = np.array(list((tfm.header.stamp.to_nsec() for tfm in self.tf_messages)))
        self.all_transform_tuples.add((orig_frame, dest_frame)) 

def thruster_ctrl_msg():
    global actuator_vel
    msg = JointState()
    msg.header = Header()
    msg.header.stamp = rospy.Time.now()
    msg.name = ['fwd']
    msg.position = [actuator_vel]
    msg.velocity = []
    msg.effort = []
    return msg 

def save_velo_data(bag, kitti, velo_frame_id, topic):
    print("Exporting velodyne data")
    velo_path = os.path.join(kitti.data_path, 'velodyne_points')
    velo_data_dir = os.path.join(velo_path, 'data')
    velo_filenames = sorted(os.listdir(velo_data_dir))
    with open(os.path.join(velo_path, 'timestamps.txt')) as f:
        lines = f.readlines()
        velo_datetimes = []
        for line in lines:
            if len(line) == 1:
                continue
            dt = datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f')
            velo_datetimes.append(dt)

    iterable = zip(velo_datetimes, velo_filenames)
    bar = progressbar.ProgressBar()
    for dt, filename in bar(iterable):
        if dt is None:
            continue

        velo_filename = os.path.join(velo_data_dir, filename)

        # read binary data
        scan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4)

        # create header
        header = Header()
        header.frame_id = velo_frame_id
        header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f")))

        # fill pcl msg
        fields = [PointField('x', 0, PointField.FLOAT32, 1),
                  PointField('y', 4, PointField.FLOAT32, 1),
                  PointField('z', 8, PointField.FLOAT32, 1),
                  PointField('i', 12, PointField.FLOAT32, 1)]
        pcl_msg = pcl2.create_cloud(header, fields, scan)

        bag.write(topic + '/pointcloud', pcl_msg, t=pcl_msg.header.stamp) 

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 save_velo_data(bag, kitti, velo_frame_id, topic):
    print("Exporting velodyne data")
    velo_path = os.path.join(kitti.data_path, 'velodyne_points')
    velo_data_dir = os.path.join(velo_path, 'data')
    velo_filenames = sorted(os.listdir(velo_data_dir))
    with open(os.path.join(velo_path, 'timestamps.txt')) as f:
        lines = f.readlines()
        velo_datetimes = []
        for line in lines:
            if len(line) == 1:
                continue
            dt = datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f')
            velo_datetimes.append(dt)

    iterable = zip(velo_datetimes, velo_filenames)
    bar = progressbar.ProgressBar()
    for dt, filename in bar(iterable):
        if dt is None:
            continue

        velo_filename = os.path.join(velo_data_dir, filename)

        # read binary data
        scan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4)

        # create header
        header = Header()
        header.frame_id = velo_frame_id
        header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f")))

        # fill pcl msg
        fields = [PointField('x', 0, PointField.FLOAT32, 1),
                  PointField('y', 4, PointField.FLOAT32, 1),
                  PointField('z', 8, PointField.FLOAT32, 1),
                  PointField('i', 12, PointField.FLOAT32, 1)]
        pcl_msg = pcl2.create_cloud(header, fields, scan)

        bag.write(topic + '/pointcloud', pcl_msg, t=pcl_msg.header.stamp) 

def test_vehicle_status(self):
        """
        Tests vehicle_status
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/vehicle_status", CarlaEgoVehicleStatus, timeout=TIMEOUT)
        self.assertNotEqual(msg.header, Header())  # todo: check frame-id
        self.assertNotEqual(msg.orientation, Quaternion()) 

def rosmsg(self):
        """:obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg 
        """
        from sensor_msgs.msg import CameraInfo, RegionOfInterest
        from std_msgs.msg import Header

        msg_header = Header()
        msg_header.frame_id = self._frame

        msg_roi = RegionOfInterest()
        msg_roi.x_offset = 0
        msg_roi.y_offset = 0
        msg_roi.height = 0
        msg_roi.width = 0
        msg_roi.do_rectify = 0

        msg = CameraInfo()
        msg.header = msg_header
        msg.height = self._height
        msg.width = self._width
        msg.distortion_model = 'plumb_bob'
        msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
        msg.K = [self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0]
        msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
        msg.P = [self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0, 0.0, 1.0, 0.0]
        msg.binning_x = 0
        msg.binning_y = 0
        msg.roi = msg_roi

        return msg 

def service_request_pose(iksvc, raw_pose, side, blocking=True):
    
    ns = "ExternalTools/"+side+"/PositionKinematicsNode/IKService"
    ikreq = SolvePositionIKRequest()
    limb = baxter_interface.Limb(side)
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
    pose_stamped = PoseStamped( header = hdr, pose = raw_pose )
    
    ikreq.pose_stamp.append(pose_stamped)
    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return 

def service_request(iksvc, desired_p, side, blocking=True):

    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
    pose = Pose(position=Point(x=desired_p[0], y=desired_p[1], z=desired_p[2]),
                orientation = Quaternion(x=desired_p[3], y=desired_p[4], z=desired_p[5], w=desired_p[6]))
            
    return service_request_pose(iksvc, pose, side, blocking) 

def service_request_velocity(iksvc, vel_vec, side):
    """Move in the requested direction at a constant velocity vector"""
    #INCOMPLETE
    ns = "ExternalTools/"+side+"/PositionKinematicsNode/IKService"
    ikreq = SolvePositionIKRequest()
    limb = baxter_interface.Limb(side)
    hdr = Header(stamp=rospy.Time.now(), frame_id='base') 

def get_forward_kinematic(joints):
    try:
        rospy.wait_for_service('compute_fk', 2)
    except (rospy.ServiceException, rospy.ROSException) as e:
        rospy.logerr("Service call failed:", e)
        return None
    try:
        moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
        fk_link = ['base_link', 'tool_link']
        joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
        header = Header(0, rospy.Time.now(), "/world")
        rs = RobotState()
        rs.joint_state.name = joint_names
        rs.joint_state.position = joints
        response = moveit_fk(header, fk_link, rs)
    except rospy.ServiceException as e:
        rospy.logerr("Service call failed:", e)
        return None

    quaternion = [response.pose_stamped[1].pose.orientation.x, response.pose_stamped[1].pose.orientation.y,
                  response.pose_stamped[1].pose.orientation.z, response.pose_stamped[1].pose.orientation.w]
    rpy = get_rpy_from_quaternion(quaternion)
    quaternion = Position.Quaternion(round(quaternion[0], 3), round(quaternion[1], 3), round(quaternion[2], 3),
                                     round(quaternion[3], 3))
    point = Position.Point(round(response.pose_stamped[1].pose.position.x, 3),
                           round(response.pose_stamped[1].pose.position.y, 3),
                           round(response.pose_stamped[1].pose.position.z, 3))
    rpy = Position.RPY(round(rpy[0], 3), round(rpy[1], 3), round(rpy[2], 3))
    rospy.loginfo("kinematic forward has been calculated ")
    return point, rpy, quaternion 

def publish_grasps_as_poses(grasps):
	rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
	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:
		p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
		grasp_PA.poses.append(p)
	grasp_publisher.publish(grasp_PA)
	rospy.loginfo('Published ' + str(len(grasp_PA.poses)) + ' poses')
	rospy.sleep(2) 

def get_header(self):
        """
        Returns ROS message header
        """
        header = Header()
        header.stamp = rospy.Time.from_sec(self.timestamp)
        return header 

def rudder_ctrl_msg(sail_ctrl):
    msg = JointState()
    msg.header = Header()
    msg.name = ['rudder_joint', 'sail_joint']
    msg.position = [0, sail_ctrl]
    msg.velocity = []
    msg.effort = []
    return msg 

def rudder_ctrl_msg():
    msg = JointState()
    msg.header = Header()
    msg.name = ['rudder_joint', 'sail_joint']
    msg.position = [0.0, sail_ctrl()]
    msg.velocity = []
    msg.effort = []
    return msg 

def sail_ctrl_msg():
    global actuator_vel
    msg = JointState()
    msg.header = Header()
    msg.name = ['fwd']
    msg.position = [sail_ctrl()]
    msg.velocity = []
    msg.effort = []
    return msg 

def rudder_ctrl_msg():
    msg = JointState()
    msg.header = Header()
    msg.name = ['rudder_joint']
    msg.position = [math.radians(rudder_ctrl())]
    msg.velocity = []
    msg.effort = []
    return msg 

def thruster_ctrl_msg():
    msg = JointState()
    msg.header = Header()
    msg.name = ['fwd_left']
    msg.position = [actuator_ctrl()]
    msg.velocity = []
    msg.effort = []
    return msg 

def thruster_ctrl_msg():
    global actuator_vel
    msg = JointState()
    msg.header = Header()
    msg.name = ['fwd']
    msg.position = [actuator_vel]
    msg.velocity = []
    msg.effort = []
    return msg 

def __init__(self):
        rospy.init_node('usv_vel_ctrl', anonymous=False)
        self.rate = 10
        r = rospy.Rate(self.rate) # 10hertz

        self.usv_vel = Odometry()
        self.usv_vel_ant = Odometry()
        self.target_vel = Twist()
        self.target_vel_ant = Twist()
        self.thruster_msg = JointState()
        self.thruster_msg.header = Header()
        self.kp_lin = 80
        self.ki_lin = 200
        thruster_name = 'fwd_left, fwd_right'
        self.I_ant_lin = 0
        self.I_ant_ang = 0
        self.lin_vel = 0
        self.lin_vel_ang = 0
        self.ang_vel = 0
        self.kp_ang = 80 
        self.ki_ang = 100
        self.thruster_max = 30
        self.vel_left = 0
        self.vel_right = 0
        self.thruster_command = numpy.array([0, 0])
        self.erro = 0

        self.pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)

        rospy.Subscriber("state", Odometry, self.get_usv_vel)  
        rospy.Subscriber("cmd_vel", Twist, self.get_target_vel)

        while not rospy.is_shutdown():
            self.pub_motor.publish(self.thruster_ctrl_msg(self.vel_ctrl(), thruster_name))
            r.sleep() 

def thruster_ctrl_msg():
    global actuator_vel
    global left
    global right
    global Kp
    msg = JointState()
    msg.header = Header()
    msg.name = ['fwd_left', 'fwd_right']
    aux = rudder_ctrl()

    left = aux + 100.
    right = -aux + 100.
    if (left < 0):
        left = 0
    if (right < 0):
        right  = 0  

    msg.position = [left, right]
    msg.velocity = []
    msg.effort = []
    return msg 

def sail_ctrl_msg():
    global actuator_vel
    msg = JointState()
    msg.header = Header()
    msg.name = ['fwd']
    msg.position = [sail_ctrl()]
    msg.velocity = []
    msg.effort = []
    return msg 

def talker():
    pub = rospy.Publisher('/barco_auv/thruster_command', JointState, queue_size=10)
    rospy.init_node('ctrl_jointState_publisher', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    hello_str = JointState()
    hello_str.header = Header()
    hello_str.name = ['fwd_left']
    hello_str.position = [10]
    hello_str.velocity = []
    hello_str.effort = []

    while not rospy.is_shutdown():
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep() 

def rudder_ctrl_msg():
    msg = JointState()
    msg.header = Header()
    msg.name = ['rudder_joint']
    msg.position = [rudder_ctrl()]
    msg.velocity = []
    msg.effort = []
    return msg 

def thruster_ctrl_msg():
    global actuator_vel
    msg = JointState()
    msg.header = Header()
    msg.name = ['fwd']
    msg.position = [actuator_vel]
    msg.velocity = []
    msg.effort = []
    return msg 

def main_fcn():
	pub = rospy.Publisher('joint_states',JointState,queue_size = 10)
	pub2 = rospy.Publisher('read_angles',Int32,queue_size = 10)
	pub3 = rospy.Publisher('uarm_status',String,queue_size = 100)

	rospy.init_node('display',anonymous = True)
	rate = rospy.Rate(20)
	

	joint_state_send = JointState()
	joint_state_send.header = Header()
	joint_state_send.name = ['base_to_base_rot','base_rot_to_link_1','link_1_to_link_2' ,'link_2_to_link_3','link_3_to_link_end']


	joint_state_send.name = joint_state_send.name+ ['link_1_to_add_1','link_2_to_add_4','link_3_to_add_2','base_rot_to_add_3','link_2_to_add_5']
	
	angle = {}

	trigger = 1

	while not rospy.is_shutdown():
		joint_state_send.header.stamp = rospy.Time.now()

		pub2.publish(1)
		if trigger == 1:
			pub3.publish("detach")
			time.sleep(1)
			trigger = 0		
				
		angle['s1'] = rospy.get_param('servo_1')*math.pi/180
		angle['s2'] = rospy.get_param('servo_2')*math.pi/180
		angle['s3'] = rospy.get_param('servo_3')*math.pi/180
		angle['s4'] = rospy.get_param('servo_4')*math.pi/180

		joint_state_send.position = [angle['s1'],angle['s2'],-angle['s2']-angle['s3'],angle['s3'],angle['s4']]
		joint_state_send.position = joint_state_send.position + [-angle['s2']-angle['s3'],angle['s3'],PI/2-angle['s3']]
		joint_state_send.position = joint_state_send.position + [angle['s2']-PI,angle['s2']+angle['s3']-PI/2]
		joint_state_send.velocity = [0]
		joint_state_send.effort = []
		
		pub.publish(joint_state_send)
		rate.sleep() 

def prediction_publish(self, idx):
        clock = Clock()

        record = np.load(os.path.join(self.npy_path, self.npy_files[idx]))
        lidar = record[:, :, :5]

        # to perform prediction
        lidar_mask = np.reshape(
            (lidar[:, :, 4] > 0),
            [self._mc.ZENITH_LEVEL, self._mc.AZIMUTH_LEVEL, 1]
        )

        norm_lidar = (lidar - self._mc.INPUT_MEAN) / self._mc.INPUT_STD

        pred_cls = self._session.run(
            self._model.pred_cls,
            feed_dict={
                self._model.lidar_input: [norm_lidar],
                self._model.keep_prob: 1.0,
                self._model.lidar_mask: [lidar_mask]
            }
        )
        label = pred_cls[0]

        # point cloud for SqueezeSeg segments
        x = lidar[:, :, 0].reshape(-1)
        y = lidar[:, :, 1].reshape(-1)
        z = lidar[:, :, 2].reshape(-1)
        i = lidar[:, :, 3].reshape(-1)
        label = label.reshape(-1)
        cloud = np.stack((x, y, z, i, label))

        header = Header()
        header.stamp = rospy.Time().now()
        header.frame_id = "velodyne_link"

        # point cloud segments
        msg_segment = self.create_cloud_xyzil32(header, cloud.T)

        # publish
        self._pub.publish(msg_segment)
        rospy.loginfo("Point cloud processed. Took %.6f ms.",
                      clock.takeRealTime())

    # create pc2_msg with 5 fields