一个新消息类创建的例子

msg文件里的point.msg

int32 x
int32 y
int32 z

节点程序

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist, Point,Quaternion
from math import copysign, sqrt, pow,radians,pi ,atan
import tf
from nav_msgs.msg import Odometry
import PyKDL
 
def quat_to_angle(quat):
    rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot.GetRPY()[2]
        
def normalize_angle(angle):
    res = angle
    while res > pi:
        res -= 2.0 * pi
    while res < -pi:
        res += 2.0 * pi
    return res

class CalibrateLinear():
    def __init__(self):
        # Give the node a name
        rospy.init_node('calibrate_linear', anonymous=False)
        
        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)
        
        # How fast will we check the odometry values?
        self.rate = rospy.get_param('~rate', 20)
        r = rospy.Rate(self.rate)
        
        x0=rospy.get_param('x0',0)
        y0=rospy.get_param('y0',0)
        x1=rospy.get_param('x1',1)
        y1=rospy.get_param('y1',1)
        #x0=0.0
        #y0=0.0
        #x1=1.0
        #y1=1.0
        d=0.1*(sqrt(pow((x1 - x0), 2) +pow((y1 - y0), 2)))
        if(x1==x0 and y1>=y0):
            angle=0
        elif(x1==x0 and y1<y0):
            angle=180
        elif(y1>y0 and x1>x0):
            angle=atan((y1-y0)/(x1-x0))*180/pi
        elif(y1>y0 and x1<x0):
            angle=(180+atan((y1-y0)/(x1-x0)))*180/pi
        elif(y1<y0 and x1<x0):
            angle=(atan((y1-y0)/(x1-x0))-180)*180/pi
        elif(y1<y0 and x1>x0):
            angle=atan((y1-y0)/(x1-x0))*180/pi
        else:
            angle=atan((y1-y0)/(x1-x0))*180/pi
        
        #rospy.loginfo('I heard %s', angle)
        # Set the distance to travel
        self.test_distance = rospy.get_param('~test_distance', 0.02) # meters
        self.linespeed = rospy.get_param('~linespeed', 0.15) # meters per second
        self.linetolerance = rospy.get_param('~linetolerance', 0.01) # meters
        self.test_angle = radians(rospy.get_param('~test_angle', 3))
        self.anglespeed = rospy.get_param('~anglespeed', 0.1) # radians per second
        self.angletolerance = radians(rospy.get_param('angletolerance', 1)) # degrees converted to radians
        self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0)
        self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0)
        self.start_test = rospy.get_param('~start_test', True)
        
        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        
 
        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
        self.base_frame = rospy.get_param('~base_frame', '/base_footprint')

        # The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        rospy.sleep(2)
        
        # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))        
            
        rospy.loginfo("Bring up rqt_reconfigure to control the test.")
            
        #move_cmd = Twist()
        reverse = 1   
        while not rospy.is_shutdown():
                if self.start_test:
            # Get the current rotation angle from tf
            self.odom_angle = self.get_odom_angle()
        
            last_angle = self.odom_angle
            turn_angle = 0
                        self.test_angle=self.test_angle-last_angle
            self.test_angle *= reverse
            error = self.test_angle - turn_angle
                        
            # Alternate directions between tests
            reverse = -reverse
        
            while abs(error) > self.angletolerance and self.start_test:
                if rospy.is_shutdown():
                return
                
                # Rotate therobot to reduce the error
                move_cmd = Twist()
                move_cmd.angular.z = copysign(self.anglespeed, error)
                self.cmd_vel.publish(move_cmd)
                r.sleep()
            
                # Get the current rotation angle from tf                   
                self.odom_angle = self.get_odom_angle()
                
                # Compute how far we have gone since the last measurement
                delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)
                
                # Add to our total angle so far
                turn_angle += delta_angle

                # Compute the new error
                error = self.test_angle - turn_angle

                # Store the current angle for the next comparison
                last_angle = self.odom_angle

                  # Stop the robot
                self.cmd_vel.publish(Twist())

              # Stop the robot by default
                move_cmd = Twist()
                self.position = Point()
        
              # Get the starting position from the tf transform between the odom and base frames
                self.position = self.get_position()
        
                x_start = self.position.x
                y_start = self.position.y
            
                if self.start_test:
                # Get the current position from the tf transform between the odom and base frames
                    self.position = self.get_position()
                
                # Compute the Euclidean distance from the target point
                    distance = sqrt(pow((self.position.x - x_start), 2) +
                                pow((self.position.y - y_start), 2))
                                
                # Correct the estimated distance by the correction factor
                    distance *= self.odom_linear_scale_correction
                
                # How close are we?
                    error =  distance - self.test_distance
                
                # Are we close enough?
                    if not self.start_test or abs(error) <  self.linetolerance:
                        self.start_test = False
                        params = {'start_test': False}
                        rospy.loginfo(params)
                    else:
                        # If not, move in the appropriate direction
                        move_cmd.linear.x = copysign(self.linespeed, -1 * error)
                else:
                    self.position = self.get_position()
                    x_start = self.position.x
                    y_start = self.position.y
                
                self.cmd_vel.publish(move_cmd)
                r.sleep()

        # Stop the robot
        self.cmd_vel.publish(Twist())

    def get_odom_angle(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return
        
        # Convert the rotation from a quaternion to an Euler angle
        return quat_to_angle(Quaternion(*rot))

    def get_position(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return Point(*trans)
        
    def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        CalibrateLinear()
        rospy.spin()
    except:
        rospy.loginfo("Calibration terminated.")