ROS里面的几个python写的工具

ROS里面的几个python写的工具

1，读取编码器 （check angular）

#!/usr/bin/env pythonimport rospy #ros的python库，python真是给力
from geometry_msgs.msg import Twist, Quaternion
from nav_msgs.msg import Odometry
import tf
from math import radians, copysign
import PyKDL
from 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 = anglewhile res > pi:res -= 2.0 * piwhile res < -pi:res += 2.0 * pireturn resclass CalibrateAngular():def __init__(self):# Give the node a namerospy.init_node('calibrate_angular', anonymous=False)# Set rospy to execute a shutdown function when terminating _shutdown(self.shutdown)# How fast will we check the odometry values?self.rate = _param('~rate', 20)r = rospy.Rate(self.rate)# The test angle is st_angle = _param('~test_angle', 360.0))self.speed = _param('~speed', 0.5) # radians lerance = _param('tolerance', 1)) # degrees converted to radiansself.odom_angular_scale_correction = _param('~odom_angular_scale_correction', 1.0)self.start_test = _param('~start_test', True)# Publisher to control the robot'd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)# The base frame is usually base_link or base_footprintself.base_frame = _param('~base_frame', '/base_footprint')# The odom frame is usually just /odomself.odom_frame = _param('~odom_frame', '/odom')# Initialize the tf listenerself.tf_listener = tf.TransformListener()# Give tf some time to fill its bufferrospy.sleep(2)# Make sure we see the odom and base framesself.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 = 1while not rospy.is_shutdown():if self.start_test:# Get the current rotation angle from tfself.odom_angle = _odom_angle()last_angle = self.odom_angleturn_angle = st_angle *= reverseerror = st_angle - turn_angle# Alternate directions between testsreverse = -reversewhile abs(error) > lerance and self.start_test:if rospy.is_shutdown():return# Rotate the robot to reduce the errormove_cmd = Twist()move_cmd.angular.z = copysign(self.speed, d_vel.publish(move_cmd)r.sleep()# Get the current rotation angle from tf                   self.odom_angle = _odom_angle()# Compute how far we have gone since the last measurementdelta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)# Add to our total angle so farturn_angle += delta_angle# Compute the new errorerror = st_angle - turn_angle# Store the current angle for the next comparisonlast_angle = self.odom_angle# Stop d_vel.publish(Twist())# Update the status flagself.start_test = Falseparams = {'start_test': False}rospy.sleep(0.5)# Stop d_vel.publish(Twist())def get_odom_angle(self):# Get the current transform between the odom and base framestry:(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 anglereturn quat_to_angle(Quaternion(*rot))def shutdown(self):# Always stop the robot when shutting down the noderospy.loginfo("Stopping ")d_vel.publish(Twist())rospy.sleep(1)if __name__ == '__main__':try:CalibrateAngular()except:rospy.loginfo("Calibration terminated.")

2，检查校正走一米

#!/usr/bin/env pythonimport rospy
from geometry_msgs.msg import Twist, Point
from math import copysign, sqrt, pow
import tfclass CalibrateLinear():def __init__(self):# Give the node a namerospy.init_node('calibrate_linear', anonymous=False)# Set rospy to execute a shutdown function when terminating _shutdown(self.shutdown)# How fast will we check the odometry values?self.rate = _param('~rate', 20)r = rospy.Rate(self.rate)# Set the distance st_distance = _param('~test_distance', 1.0) # metersself.speed = _param('~speed', 0.15) # meters lerance = _param('~tolerance', 0.01) # metersself.odom_linear_scale_correction = _param('~odom_linear_scale_correction', 1.0)self.start_test = _param('~start_test', True)# Publisher to control the robot'd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)# The base frame is base_footprint for the TurtleBot but base_link for Pi Robotself.base_frame = _param('~base_frame', '/base_footprint')# The odom frame is usually just /odomself.odom_frame = _param('~odom_frame', '/odom')# Initialize the tf listenerself.tf_listener = tf.TransformListener()# Give tf some time to fill its bufferrospy.sleep(2)# Make sure we see the odom and base framesself.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 framesself.position = _position()x_start = _start = ve_cmd = Twist()while not rospy.is_shutdown():# Stop the robot by defaultmove_cmd = Twist()if self.start_test:# Get the current position from the tf transform between the odom and base framesself.position = _position()# Compute the Euclidean distance from the target pointdistance = sqrt(pow((self.position.x - x_start), 2) +pow((self.position.y - y_start), 2))# Correct the estimated distance by the correction factordistance *= self.odom_linear_scale_correction# How close are we?error =  distance - st_distance# Are we close enough?if not self.start_test or abs(error) <  lerance:self.start_test = Falseparams = {'start_test': False}rospy.loginfo(params)else:# If not, move in the appropriate directionmove_cmd.linear.x = copysign(self.speed, -1 * error)else:self.position = _position()x_start = _start = self.d_vel.publish(move_cmd)r.sleep()# Stop d_vel.publish(Twist())def get_position(self):# Get the current transform between the odom and base framestry:(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")returnreturn Point(*trans)def shutdown(self):# Always stop the robot when shutting down the noderospy.loginfo("Stopping ")d_vel.publish(Twist())rospy.sleep(1)if __name__ == '__main__':try:CalibrateLinear()rospy.spin()except:rospy.loginfo("Calibration terminated.")

3，键盘控制

#!/usr/bin/env python
import 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    oj    k    lm    ,    .
For Holonomic mode (strafing), hold down the shift key:
---------------------------U    I    OJ    K    LM    <    >
t : up (+z)
b : down (-z)
anything else : stop
q/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 = ad(setattr(sys.stdin, termios.TCSADRAIN, settings)return keyspeed = 0.30
turn = 0.6def vels(speed,turn):return "currently:tspeed %stturn %s " % (speed,turn)if __name__=="__main__":settings = attr(sys.stdin)pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)rospy.init_node('teleop_twist_keyboard')x = 0y = 0z = 0th = 0status = 0try:print msgprint 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 msgstatus = (status + 1) % 15else:x = 0y = 0z = 0th = 0if (key == 'x03'):breaktwist = 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*turnpub.publish(twist)except:print efinally:twist = Twist()twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0pub.publish(setattr(sys.stdin, termios.TCSADRAIN, settings)