ROS里面的几个python写的工具

1，读取编码器 （check angular）

#!/usr/bin/env pythonimport rospy #ros的python库，python真是给力from geometry_msgs.msg import Twist, Quaternionfrom nav_msgs.msg import Odometryimport tffrom math import radians, copysignimport PyKDLfrom math import pidef 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 resclass CalibrateAngular():    def __init__(self):        # Give the node a name        rospy.init_node('calibrate_angular', 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)        # The test angle is 360 degrees        self.test_angle = radians(rospy.get_param('~test_angle', 360.0))        self.speed = rospy.get_param('~speed', 0.5) # radians per second        self.tolerance = radians(rospy.get_param('tolerance', 1)) # degrees converted to radians        self.odom_angular_scale_correction = rospy.get_param('~odom_angular_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 usually base_link or base_footprint        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.")        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 *= reverse                error = self.test_angle - turn_angle                # Alternate directions between tests                reverse = -reverse                while abs(error) > self.tolerance and self.start_test:                    if rospy.is_shutdown():                        return                    # Rotate the robot to reduce the error                    move_cmd = Twist()                    move_cmd.angular.z = copysign(self.speed, 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())                # Update the status flag                self.start_test = False                params = {'start_test': False}            rospy.sleep(0.5)        # 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 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:        CalibrateAngular()    except:        rospy.loginfo("Calibration terminated.")

2，检查校正走一米

#!/usr/bin/env pythonimport rospyfrom geometry_msgs.msg import Twist, Pointfrom math import copysign, sqrt, powimport tfclass 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)        # Set the distance to travel        self.test_distance = rospy.get_param('~test_distance', 1.0) # meters        self.speed = rospy.get_param('~speed', 0.15) # meters per second        self.tolerance = rospy.get_param('~tolerance', 0.01) # meters        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.")        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        move_cmd = Twist()        while not rospy.is_shutdown():            # Stop the robot by default            move_cmd = Twist()            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.tolerance:                    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.speed, -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_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.")

3，键盘控制

#!/usr/bin/env pythonimport roslib; roslib.load_manifest('teleop_twist_keyboard')import rospyfrom geometry_msgs.msg import Twistimport sys, select, termios, ttymsg = """Reading from the keyboard  and Publishing to Twist!---------------------------Moving around:   u    i    o   j    k    l   m    ,    .For Holonomic mode (strafing), hold down the shift key:---------------------------   U    I    O   J    K    L   M    <    >t : up (+z)b : down (-z)anything else : stopq/z : increase/decrease max speeds by 10%w/x : increase/decrease only linear speed by 10%e/c : increase/decrease only angular speed by 10%CTRL-C to quit"""moveBindings = {        'i':(1,0,0,0),        'o':(1,0,0,-1),        'j':(0,0,0,1),        'l':(0,0,0,-1),        'u':(1,0,0,1),        ',':(-1,0,0,0),        '.':(-1,0,0,1),        'm':(-1,0,0,-1),        'O':(1,-1,0,0),        'I':(1,0,0,0),        'J':(0,1,0,0),        'L':(0,-1,0,0),        'U':(1,1,0,0),        '<':(-1,0,0,0),        '>':(-1,-1,0,0),        'M':(-1,1,0,0),        't':(0,0,1,0),        'b':(0,0,-1,0),           }speedBindings={        'q':(1.1,1.1),        'z':(.9,.9),        'w':(1.1,1),        'x':(.9,1),        'e':(1,1.1),        'c':(1,.9),          }def getKey():    tty.setraw(sys.stdin.fileno())    select.select([sys.stdin], [], [], 0)    key = sys.stdin.read(1)    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)    return keyspeed = 0.30turn = 0.6def vels(speed,turn):    return "currently:\tspeed %s\tturn %s " % (speed,turn)if __name__=="__main__":        settings = termios.tcgetattr(sys.stdin)    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)    rospy.init_node('teleop_twist_keyboard')    x = 0    y = 0    z = 0    th = 0    status = 0    try:        print msg        print vels(speed,turn)        while(1):            key = getKey()            if key in moveBindings.keys():                x = moveBindings[key][0]                y = moveBindings[key][1]                z = moveBindings[key][2]                th = moveBindings[key][3]            elif key in speedBindings.keys():                speed = speed * speedBindings[key][0]                turn = turn * speedBindings[key][1]                print vels(speed,turn)                if (status == 14):                    print msg                status = (status + 1) % 15            else:                x = 0                y = 0                z = 0                th = 0                if (key == '\x03'):                    break            twist = Twist()            twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn            pub.publish(twist)    except:        print e    finally:        twist = Twist()        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0        pub.publish(twist)            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)