Python msg.Odometry方法代码示例

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def talker_ctrl():
    global rate_value
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("usv_position_setpoint", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        pub_motor.publish(thruster_ctrl_msg())
        pub_rudder.publish(rudder_ctrl_msg())
        rate.sleep() 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def talker_ctrl():
    global rate_value
    global result
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:    
            pub_motor.publish(thruster_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def publish_ref(self, *args):
        """
        Publishes the reference trajectory as an Odometry message,
        and reference effort as a WrenchStamped message.

        """
        # Make sure a plan exists
        last_update_time = self.last_update_time
        if self.get_ref is None or last_update_time is None:
            return

        # Time since last update
        T = self.rostime() - last_update_time
        stamp = rospy.Time.now()

        # Publish interpolated reference
        self.ref_pub.publish(self.pack_odom(self.get_ref(T), stamp))

        # Not really necessary, but for fun-and-profit also publish the planner's effort wrench
        if self.get_eff is not None:
            self.eff_pub.publish(self.pack_wrenchstamped(self.get_eff(T), stamp)) 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def odom_cb(self, msg):
        """
        Expects an Odometry message.
        Stores the current state of the vehicle tracking the plan.
        Reference frame information is also recorded.
        Determines if the vehicle is tracking well.

        """
        self.world_frame_id = msg.header.frame_id
        self.body_frame_id = msg.child_frame_id
        self.state = self.unpack_odom(msg)
        last_update_time = self.last_update_time
        if self.get_ref is not None and last_update_time is not None:
            if np.all(np.abs(self.erf(self.get_ref(self.rostime() - last_update_time), self.state)) < 2*np.array(params.real_tol)):
                self.tracking = True
            else:
                self.tracking = False

################################################# CONVERTERS 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def _publish_odometry(self):
        """
        Publish current pose as Odometry message.

        """
        # only publish if we have a subscriber
        if self._odom_pub.get_num_connections() == 0:
            return

        now = rospy.Time.now()
        odom = Odometry()
        odom.header.frame_id = self._odom_frame
        odom.header.stamp = now
        odom.child_frame_id = self._footprint_frame
        odom.pose.pose.position.x = self._cozmo.pose.position.x * 0.001
        odom.pose.pose.position.y = self._cozmo.pose.position.y * 0.001
        odom.pose.pose.position.z = self._cozmo.pose.position.z * 0.001
        q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
        odom.pose.pose.orientation.x = q[0]
        odom.pose.pose.orientation.y = q[1]
        odom.pose.pose.orientation.z = q[2]
        odom.pose.pose.orientation.w = q[3]
        odom.pose.covariance = np.diag([1e-2, 1e-2, 1e-2, 1e3, 1e3, 1e-1]).ravel()
        odom.twist.twist.linear.x = self._lin_vel
        odom.twist.twist.angular.z = self._ang_vel
        odom.twist.covariance = np.diag([1e-2, 1e3, 1e3, 1e3, 1e3, 1e-2]).ravel()
        self._odom_pub.publish(odom) 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def goal_pose(pose):
    goal_pose = Odometry()
    goal_pose.header.stamp = rospy.Time.now()
    goal_pose.header.frame_id = 'world'
    goal_pose.pose.pose.position = Point(pose[0][0]+x_offset, pose[0][1]+y_offset, 0.)
    return goal_pose 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def talker_ctrl():
    global rate_value
    global currentHeading
    global windDir 
    global isTacking
    global heeling
    global spHeading

    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    pub_heading = rospy.Publisher('currentHeading', Float64, queue_size=10)
    pub_windDir = rospy.Publisher('windDirection', Float64, queue_size=10)
    pub_heeling = rospy.Publisher('heeling', Float64, queue_size=10)
    pub_spHeading = rospy.Publisher('spHeading', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:
            pub_rudder.publish(rudder_ctrl_msg())
            #pub_sail.publish(sail_ctrl())
            pub_result.publish(verify_result())
            pub_heading.publish(currentHeading)
            pub_windDir.publish(windDir)
            pub_heeling.publish(heeling)
            pub_spHeading.publish(spHeading)
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def navigation():
    rospy.Publisher('usv_position_setpoint', Odometry) # only create a rostopic, navigation system TODO
    rospy.spin()

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

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

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

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def talker_ctrl():
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('/barco_auv/thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('/barco_auv/joint_setpoint', JointState, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(10) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("/barco_auv/state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("usv_position_setpoint", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        pub_motor.publish(thruster_ctrl_msg())
        pub_rudder.publish(rudder_ctrl_msg())
        rate.sleep() 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import Odometry [as 别名]
def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('odom', Odometry, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      rospy.sleep(0.3)
      rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926))