基于测量的前进返回脚本

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, piclass OutAndBack():def __init__(self):# Give the node a namerospy.init_node('out_and_back', anonymous=False)# Set rospy to execute a shutdown function when exiting       
        rospy.on_shutdown(self.shutdown)# Publisher to control the robot's speedself.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)# How fast will we update the robot's movement?rate = 20# Set the equivalent ROS rate variabler = rospy.Rate(rate)# Set the forward linear speed to 0.15 meters per second linear_speed = 0.15# Set the travel distance in metersgoal_distance = 1.0# Set the rotation speed in radians per secondangular_speed = 0.5# Set the angular tolerance in degrees converted to radiansangular_tolerance = radians(1.0)# Set the rotation angle to Pi radians (180 degrees)goal_angle = pi# Initialize the tf listenerself.tf_listener = tf.TransformListener()# Give tf some time to fill its bufferrospy.sleep(2)# Set the odom frameself.odom_frame = '/odom'# Find out if the robot uses /base_link or /base_footprinttry:self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))self.base_frame = '/base_footprint'except (tf.Exception, tf.ConnectivityException, tf.LookupException):try:self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))self.base_frame = '/base_link'except (tf.Exception, tf.ConnectivityException, tf.LookupException):rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")rospy.signal_shutdown("tf Exception")  # Initialize the position variable as a Point typeposition = Point()# Loop once for each leg of the tripfor i in range(2):# Initialize the movement commandmove_cmd = Twist()# Set the movement command to forward motionmove_cmd.linear.x = linear_speed# Get the starting position values     (position, rotation) = self.get_odom()x_start = position.xy_start = position.y# Keep track of the distance traveleddistance = 0# Enter the loop to move along a sidewhile distance < goal_distance and not rospy.is_shutdown():# Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)r.sleep()# Get the current position(position, rotation) = self.get_odom()# Compute the Euclidean distance from the startdistance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2))# Stop the robot before the rotationmove_cmd = Twist()self.cmd_vel.publish(move_cmd)rospy.sleep(1)# Set the movement command to a rotationmove_cmd.angular.z = angular_speed# Track the last angle measuredlast_angle = rotation# Track how far we have turnedturn_angle = 0while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():# Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)r.sleep()# Get the current rotation(position, rotation) = self.get_odom()# Compute the amount of rotation since the last loopdelta_angle = normalize_angle(rotation - last_angle)# Add to the running totalturn_angle += delta_anglelast_angle = rotation# Stop the robot before the next legmove_cmd = Twist()self.cmd_vel.publish(move_cmd)rospy.sleep(1)# Stop the robot for good
        self.cmd_vel.publish(Twist())def get_odom(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), 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:OutAndBack()except:rospy.loginfo("Out-and-Back node terminated.")