ROS里面的几个python写的工具

#!/usr/bin/env python

import 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 pi

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 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.")

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist, Point
from math import copysign, sqrt, pow
import tf

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)

# 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.")

#!/usr/bin/env python
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy

from geometry_msgs.msg import Twist

import sys, select, termios, tty

msg = """
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 : 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 = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key

speed = 0.30
turn = 0.6

def 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)