move_base_straight.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 '''
00004 Created on 01.08.2013
00005 
00006 @author: Thorsten Gedicke
00007 '''
00008 
00009 import roslib; roslib.load_manifest('move_base_straight')
00010 import rospy
00011 import numpy as np
00012 import tf
00013 import actionlib
00014 from geometry_msgs.msg import PoseStamped, Twist
00015 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00016 from sensor_msgs.msg import LaserScan
00017 
00018 class MoveBaseStraightAction(object):
00019 
00020     FREQ = 10
00021 
00022     def __init__(self, name):
00023         # Get parameters -------------------------------------------------------
00024         # Max and min speeds
00025         self.MAX_SPEED = rospy.get_param('~max_speed', 0.2) # [m/s]
00026         self.MIN_SPEED = rospy.get_param('~min_speed', 0.05) # [m/s]
00027 
00028         # multiplier for turning ratio; e.g., 0.6 = rotate at 60% of the speed
00029         # required to fully turn towards the goal in 1 second
00030         self.ANGULAR_SPEED_GAIN = rospy.get_param('~angular_speed', 0.6)
00031 
00032         # Tolerance for goal approach (we stop when distance is below this)
00033         self.GOAL_THRESHOLD = rospy.get_param('~goal_threshold', 0.02) # [m]
00034 
00035         # Tolerance for movement success (action returns aborted if
00036         # distance to goal is still above this after we stopped)
00037         self.GOAL_THRESHOLD_ACCEPTABLE = rospy.get_param('~goal_threshold_acceptable', 0.2) # [m]
00038 
00039         self.YAW_GOAL_TOLERANCE = rospy.get_param('~yaw_goal_tolerance', 0.2) # [rad]
00040 
00041         # Minimal safety radius around base footprint
00042         self.RANGE_MINIMUM = rospy.get_param('~range_minimum', 0.5) # [m]
00043 
00044         # We start to slow down if distance to goal or obstacle distance to
00045         # RANGE_MINIMUM is below SLOWDOWN_RANGE.
00046         self.SLOWDOWN_RANGE = rospy.get_param('~slowdown_range', 0.5) # [m]
00047 
00048         # Scanned aperture around the goal for a movement to be cosidered safe.
00049         # At this time, there is no effect for this parameter since nothing
00050         # special is done for unsafe movements.
00051         self.REQUIRED_APERTURE = rospy.get_param('~required_aperture', np.pi * 0.75) # [rad]
00052 
00053         # Is the robot holonomic?
00054         self.HOLONOMIC = rospy.get_param('~holonomic', False)
00055 
00056         # We can also listen for PoseStamped targets on some topic.
00057         self.GOAL_TOPIC_NAME = rospy.get_param('~goal_topic_name', None)
00058         # ----------------------------------------------------------------------
00059 
00060         self.action_name = name
00061         self.tf_listener = tf.TransformListener()
00062         self.cmd_vel_pub = rospy.Publisher('base_controller/command', Twist, queue_size=1)
00063 
00064         self.speed_multiplier = 1.0
00065 
00066         # Subscribe to base_scan
00067         self.scan = None
00068         self.laser_base_offset = None    # will be initialized from first scan message
00069         rospy.Subscriber("/base_scan", LaserScan, self.laser_cb)
00070 
00071         self.footprint_frame = rospy.get_param('~footprint_frame', '/base_footprint')
00072 
00073         # Set up action server
00074         self.action_server = actionlib.SimpleActionServer(self.action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start=False)
00075         self.action_server.start()
00076         rospy.loginfo('%s: Action server up and running...' % (self.action_name))
00077 
00078         if self.GOAL_TOPIC_NAME:
00079             self.client = actionlib.SimpleActionClient('move_base_straight', MoveBaseAction)
00080             self.client.wait_for_server()
00081             rospy.Subscriber(self.GOAL_TOPIC_NAME, PoseStamped, self.manual_cb)
00082             rospy.loginfo('%s: Manual control topic is: %s' % (self.action_name, self.GOAL_TOPIC_NAME))
00083 
00084     def laser_to_base(self, distance_laser, angle_laser):
00085         """
00086         Uses the laws of sines and cosines to transform range/angle pairs in the
00087         base laser frame to range/angle pairs relative to the base footprint frame.
00088         """
00089         gamma = np.pi - angle_laser
00090         distance_base = np.sqrt(self.laser_base_offset**2 + distance_laser**2 - 2 * self.laser_base_offset * distance_laser * np.cos(gamma))
00091         angle_base = np.arcsin(distance_laser * np.sin(gamma) / distance_base)
00092         return (distance_base, angle_base)
00093 
00094     def blocked(self, target_angle, dist):
00095         # Something in the way?
00096         blocked = False
00097         block_reason = None
00098         # used to reduce speed for goal/obstacle approach
00099         self.speed_multiplier = min(1.0, (dist - self.GOAL_THRESHOLD) / self.SLOWDOWN_RANGE)
00100         laser_angle = self.scan.angle_min
00101         for laser_distance in self.scan.ranges:
00102             (base_distance, base_angle) = self.laser_to_base(laser_distance, laser_angle)
00103             angle_diff = abs(base_angle - target_angle)
00104             will_get_closer = angle_diff < (np.pi / 2.0)
00105             is_too_close = (base_distance < self.RANGE_MINIMUM) and (laser_distance > self.scan.range_min)
00106             if will_get_closer:
00107                 self.speed_multiplier = min(self.speed_multiplier, (base_distance - self.RANGE_MINIMUM) / self.SLOWDOWN_RANGE)
00108             if is_too_close and will_get_closer:
00109                 blocked = True
00110                 block_reason = (base_distance, base_angle, angle_diff)
00111                 break
00112             laser_angle += self.scan.angle_increment
00113         if blocked:
00114             rospy.logwarn('%s: Blocked! Reason: Distance %s at angle %s (%s angle difference to goal direction)' % (self.action_name, block_reason[0], block_reason[1], block_reason[2]))
00115         return blocked
00116 
00117 
00118     def translate_towards_goal_holonomically(self, x_diff, y_diff, dist):
00119         drive_speed = max(self.MAX_SPEED * self.speed_multiplier, self.MIN_SPEED)
00120         cmd = Twist()
00121         cmd.linear.x = (x_diff / dist) * drive_speed
00122         cmd.linear.y = (y_diff / dist) * drive_speed
00123         self.cmd_vel_pub.publish(cmd)
00124 
00125 
00126     def translate_towards_goal(self, target_angle):
00127         # Drive towards goal
00128         cmd = Twist()
00129         cmd.linear.x = max(self.MAX_SPEED * self.speed_multiplier, self.MIN_SPEED)
00130         cmd.angular.z = target_angle * self.ANGULAR_SPEED_GAIN
00131         self.cmd_vel_pub.publish(cmd)
00132 
00133     def rotate_in_place(self, direction):
00134         # Rotate towards goal orientation
00135         cmd = Twist()
00136         if (direction > 0):
00137             cmd.angular.z = 0.2
00138         else:
00139             cmd.angular.z = -0.2
00140         self.cmd_vel_pub.publish(cmd)
00141 
00142     def laser_cb(self, scan):
00143         if not self.laser_base_offset:
00144             # Get laser to base_footprint frame offset
00145             laser_frame = scan.header.frame_id
00146             while not rospy.is_shutdown():
00147                 try:
00148                     self.tf_listener.waitForTransform(self.footprint_frame, laser_frame, rospy.Time(), rospy.Duration(1.0))
00149                     self.laser_base_offset = self.tf_listener.lookupTransform(self.footprint_frame, laser_frame, rospy.Time())[0][0]
00150                     break
00151                 except (tf.Exception) as e:
00152                     rospy.logwarn("MoveBaseBlind tf exception! Message: %s" % e.message)
00153                     rospy.sleep(0.1)
00154                     continue
00155 
00156         self.scan = scan
00157 
00158 
00159     def manual_cb(self, target_pose):
00160         goal = MoveBaseGoal(target_pose=target_pose)
00161         self.client.send_goal(goal)
00162 
00163     def execute_cb(self, goal):
00164         if not self.scan or not self.laser_base_offset:
00165             rospy.logwarn('No messages received yet on topic %s, aborting goal!' % rospy.resolve_name('base_scan'))
00166             self.action_server.set_aborted()
00167             return
00168 
00169         # helper variables
00170         target_pose = goal.target_pose
00171 
00172         # publish info to the console
00173         rospy.loginfo('%s: Executing, moving to position: (%f, %f)' % (self.action_name, target_pose.pose.position.x, target_pose.pose.position.y))
00174 
00175         rate = rospy.Rate(hz = MoveBaseStraightAction.FREQ)
00176         while not rospy.is_shutdown():
00177             rate.sleep()
00178             # get transform relative to /base_footprint (--> target_pose_transformed)
00179             while not rospy.is_shutdown() and not self.action_server.is_preempt_requested():
00180                 try:
00181                     # set the time stamp to now so we actually check for the
00182                     # same transform in waitForTransform and transformPose.
00183                     target_pose.header.stamp = rospy.Time.now()
00184 
00185                     self.tf_listener.waitForTransform('/base_footprint',
00186                             target_pose.header.frame_id,
00187                             target_pose.header.stamp, rospy.Duration(1.0))
00188                     target_pose_transformed = self.tf_listener.transformPose('/base_footprint', target_pose)
00189                     break
00190                 except (tf.Exception) as e:
00191                     rospy.logwarn("MoveBaseBlind tf exception! Message: %s" % e.message)
00192                     rospy.sleep(0.1)
00193                     continue
00194 
00195             # check that preempt has not been requested by the client
00196             if self.action_server.is_preempt_requested():
00197                 rospy.loginfo('%s: Preempted' % self.action_name)
00198                 self.action_server.set_preempted()
00199                 break
00200 
00201             x_diff = target_pose_transformed.pose.position.x
00202             y_diff = target_pose_transformed.pose.position.y
00203             dist = np.sqrt(x_diff ** 2 + y_diff ** 2)
00204 
00205             euler = tf.transformations.euler_from_quaternion([target_pose_transformed.pose.orientation.x,
00206                     target_pose_transformed.pose.orientation.y, target_pose_transformed.pose.orientation.z,
00207                     target_pose_transformed.pose.orientation.w])
00208 
00209             target_angle = np.arctan2(target_pose_transformed.pose.position.y,
00210                                       target_pose_transformed.pose.position.x)
00211             # target_angle is 0 in the center of the laser scan
00212 
00213             # Check if path is blocked. If so, abort.
00214             if self.blocked(target_angle, dist):
00215                 # send command to stop
00216                 self.cmd_vel_pub.publish(Twist())
00217                 if(dist <= self.GOAL_THRESHOLD_ACCEPTABLE):
00218                     rospy.loginfo('%s: Succeeded (blocked, but closer to goal than acceptable goal threshold)' % self.action_name)
00219                     self.action_server.set_succeeded()
00220                 else:
00221                     rospy.logwarn('%s: Aborted' % self.action_name)
00222                     self.action_server.set_aborted()
00223                 break
00224 
00225             # Can we see enough?
00226             if ((target_angle - self.REQUIRED_APERTURE/2) < self.scan.angle_min or
00227                 (target_angle + self.REQUIRED_APERTURE/2) > self.scan.angle_max):
00228                 # Driving blind (maybe activate warning signals here)
00229                 pass
00230 
00231             # Goal not yet reached?
00232             # Translate holonomically
00233             if (self.HOLONOMIC and dist > self.GOAL_THRESHOLD):
00234                 self.translate_towards_goal_holonomically(x_diff, y_diff, dist)
00235 
00236             # Translate non-holonomically
00237             elif (dist > self.GOAL_THRESHOLD):
00238                 self.translate_towards_goal(target_angle)
00239 
00240             # If arrived, but yaw error too large, stop and rotate
00241             elif (abs(euler[2]) > self.YAW_GOAL_TOLERANCE):
00242                 self.rotate_in_place(euler[2])
00243 
00244             # Arrived
00245             else:
00246                 # send command to stop
00247                 self.cmd_vel_pub.publish(Twist())
00248                 rospy.loginfo('%s: Succeeded' % self.action_name)
00249                 self.action_server.set_succeeded()
00250                 break
00251 
00252 if __name__ == '__main__':
00253     rospy.init_node('move_base_straight')
00254     MoveBaseStraightAction(rospy.get_name())
00255     rospy.spin()


move_base_straight
Author(s): Thorsten Gedicke
autogenerated on Wed May 24 2017 03:02:49