SHARE
TWEET

Untitled

a guest Feb 14th, 2020 68 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2.  
  3. from __future__ import absolute_import
  4. from __future__ import division
  5. from __future__ import print_function
  6.  
  7. import argparse
  8. import numpy as np
  9. import os
  10. import rospy
  11. import sys
  12.  
  13. # Robot motion commands:
  14. # http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html
  15. from geometry_msgs.msg import Twist
  16. # Occupancy grid.
  17. from nav_msgs.msg import OccupancyGrid
  18. # Position.
  19. from tf import TransformListener
  20. # Goal.
  21. from geometry_msgs.msg import PoseStamped
  22. # Path.
  23. from nav_msgs.msg import Path
  24. # For pose information.
  25. from tf.transformations import euler_from_quaternion
  26.  
  27. # Import the potential_field.py code rather than copy-pasting.
  28. directory = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../python')
  29. sys.path.insert(0, directory)
  30. try:
  31.   import rrt
  32. except ImportError:
  33.   raise ImportError('Unable to import potential_field.py. Make sure this file is in "{}"'.format(directory))
  34.  
  35.  
  36. SPEED = .2
  37. EPSILON = .1
  38.  
  39. X = 0
  40. Y = 1
  41. YAW = 2
  42.  
  43. ERRORS_OVER_TIME = 0.
  44. LAST_ERROR = 0.
  45.  
  46.  
  47. def feedback_linearized(pose, velocity, epsilon):
  48.   u = 0.  # [m/s]
  49.   w = 0.  # [rad/s] going counter-clockwise.
  50.  
  51.   # MISSING: Implement feedback-linearization to follow the velocity
  52.   # vector given as argument. Epsilon corresponds to the distance of
  53.   # linearized point in front of the robot.
  54.   u = velocity[X] * np.cos(pose[YAW]) + velocity[Y] * np.sin(pose[YAW])
  55.   w = 1. / epsilon * (-velocity[X] * np.sin(pose[YAW]) + velocity[Y] * np.cos(pose[YAW]))
  56.  
  57.   return u, w
  58.  
  59.  
  60. def get_velocity(position, path_points):
  61.   v = np.zeros_like(position)
  62.   if len(path_points) == 0:
  63.     return v
  64.   # Stop moving if the goal is reached.
  65.   if np.linalg.norm(position - path_points[-1]) < .2:
  66.     return v
  67.  
  68.   # MISSING: Return the velocity needed to follow the
  69.   # path defined by path_points. Assume holonomicity of the
  70.   # point located at position.
  71.  
  72.   global ERRORS_OVER_TIME
  73.   global LAST_ERROR
  74.  
  75.   next_closest_path_point_index = 0
  76.   min_dist = np.linalg.norm(position - path_points[0])
  77.   for i in range(1, len(path_points)):
  78.     path_point = path_points[i]
  79.     dist = np.linalg.norm(position - path_point)
  80.     if dist < min_dist:
  81.       next_closest_path_point_index = i
  82.       min_dist = dist
  83.  
  84.   if next_closest_path_point_index == len(path_points) - 1:
  85.     next_closest_path_point = path_points[-1]
  86.   else:
  87.     next_closest_path_point = path_points[next_closest_path_point_index + 1]
  88.   error = next_closest_path_point - position
  89.   v = 3. * error + .03 * ERRORS_OVER_TIME + .02 * (error - LAST_ERROR)
  90.   ERRORS_OVER_TIME += error
  91.   LAST_ERROR = error
  92.  
  93.   return v
  94.  
  95.  
  96. class SLAM(object):
  97.   def __init__(self):
  98.     rospy.Subscriber('/map', OccupancyGrid, self.callback)
  99.     self._tf = TransformListener()
  100.     self._occupancy_grid = None
  101.     self._pose = np.array([np.nan, np.nan, np.nan], dtype=np.float32)
  102.    
  103.   def callback(self, msg):
  104.     values = np.array(msg.data, dtype=np.int8).reshape((msg.info.width, msg.info.height))
  105.     processed = np.empty_like(values)
  106.     processed[:] = rrt.FREE
  107.     processed[values < 0] = rrt.UNKNOWN
  108.     processed[values > 50] = rrt.OCCUPIED
  109.     processed = processed.T
  110.     origin = [msg.info.origin.position.x, msg.info.origin.position.y, 0.]
  111.     resolution = msg.info.resolution
  112.     self._occupancy_grid = rrt.OccupancyGrid(processed, origin, resolution)
  113.  
  114.   def update(self):
  115.     # Get pose w.r.t. map.
  116.     a = 'occupancy_grid'
  117.     b = 'base_link'
  118.     if self._tf.frameExists(a) and self._tf.frameExists(b):
  119.       try:
  120.         t = rospy.Time(0)
  121.         position, orientation = self._tf.lookupTransform('/' + a, '/' + b, t)
  122.         self._pose[X] = position[X]
  123.         self._pose[Y] = position[Y]
  124.         _, _, self._pose[YAW] = euler_from_quaternion(orientation)
  125.       except Exception as e:
  126.         print(e)
  127.     else:
  128.       print('Unable to find:', self._tf.frameExists(a), self._tf.frameExists(b))
  129.     pass
  130.  
  131.   @property
  132.   def ready(self):
  133.     return self._occupancy_grid is not None and not np.isnan(self._pose[0])
  134.  
  135.   @property
  136.   def pose(self):
  137.     return self._pose
  138.  
  139.   @property
  140.   def occupancy_grid(self):
  141.     return self._occupancy_grid
  142.  
  143.  
  144. class GoalPose(object):
  145.   def __init__(self):
  146.     rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.callback)
  147.     self._position = np.array([np.nan, np.nan], dtype=np.float32)
  148.  
  149.   def callback(self, msg):
  150.     # The pose from RViz is with respect to the "map".
  151.     self._position[X] = msg.pose.position.x
  152.     self._position[Y] = msg.pose.position.y
  153.     print('Received new goal position:', self._position)
  154.  
  155.   @property
  156.   def ready(self):
  157.     return not np.isnan(self._position[0])
  158.  
  159.   @property
  160.   def position(self):
  161.     return self._position
  162.  
  163.  
  164. def get_path(final_node):
  165.   # Construct path from RRT solution.
  166.   if final_node is None:
  167.     return []
  168.   path_reversed = []
  169.   path_reversed.append(final_node)
  170.   while path_reversed[-1].parent is not None:
  171.     path_reversed.append(path_reversed[-1].parent)
  172.   path = list(reversed(path_reversed))
  173.   # Put a point every 5 cm.
  174.   distance = 0.05
  175.   offset = 0.
  176.   points_x = []
  177.   points_y = []
  178.   for u, v in zip(path, path[1:]):
  179.     center, radius = rrt.find_circle(u, v)
  180.     du = u.position - center
  181.     theta1 = np.arctan2(du[1], du[0])
  182.     dv = v.position - center
  183.     theta2 = np.arctan2(dv[1], dv[0])
  184.     # Check if the arc goes clockwise.
  185.     clockwise = np.cross(u.direction, du).item() > 0.
  186.     # Generate a point every 5cm apart.
  187.     da = distance / radius
  188.     offset_a = offset / radius
  189.     if clockwise:
  190.       da = -da
  191.       offset_a = -offset_a
  192.       if theta2 > theta1:
  193.         theta2 -= 2. * np.pi
  194.     else:
  195.       if theta2 < theta1:
  196.         theta2 += 2. * np.pi
  197.     angles = np.arange(theta1 + offset_a, theta2, da)
  198.     offset = distance - (theta2 - angles[-1]) * radius
  199.     points_x.extend(center[X] + np.cos(angles) * radius)
  200.     points_y.extend(center[Y] + np.sin(angles) * radius)
  201.   return zip(points_x, points_y)
  202.  
  203.  
  204. def run(args):
  205.   rospy.init_node('rrt_navigation')
  206.  
  207.   # Update control every 100 ms.
  208.   rate_limiter = rospy.Rate(100)
  209.   publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
  210.   path_publisher = rospy.Publisher('/path', Path, queue_size=1)
  211.   slam = SLAM()
  212.   goal = GoalPose()
  213.   frame_id = 0
  214.   current_path = []
  215.   previous_time = rospy.Time.now().to_sec()
  216.  
  217.   # Stop moving message.
  218.   stop_msg = Twist()
  219.   stop_msg.linear.x = 0.
  220.   stop_msg.angular.z = 0.
  221.  
  222.   # Make sure the robot is stopped.
  223.   i = 0
  224.   while i < 10 and not rospy.is_shutdown():
  225.     publisher.publish(stop_msg)
  226.     rate_limiter.sleep()
  227.     i += 1
  228.  
  229.   while not rospy.is_shutdown():
  230.     slam.update()
  231.     current_time = rospy.Time.now().to_sec()
  232.  
  233.     # Make sure all measurements are ready.
  234.      # Get map and current position through SLAM:
  235.     # > roslaunch exercises slam.launch
  236.     if not goal.ready or not slam.ready:
  237.       rate_limiter.sleep()
  238.       continue
  239.  
  240.     goal_reached = np.linalg.norm(slam.pose[:2] - goal.position) < .2
  241.     if goal_reached:
  242.       publisher.publish(stop_msg)
  243.       rate_limiter.sleep()
  244.       continue
  245.  
  246.     # Follow path using feedback linearization.
  247.     position = np.array([
  248.         slam.pose[X] + EPSILON * np.cos(slam.pose[YAW]),
  249.         slam.pose[Y] + EPSILON * np.sin(slam.pose[YAW])], dtype=np.float32)
  250.     v = get_velocity(position, np.array(current_path, dtype=np.float32))
  251.     u, w = feedback_linearized(slam.pose, v, epsilon=EPSILON)
  252.     vel_msg = Twist()
  253.     vel_msg.linear.x = u
  254.     vel_msg.angular.z = w
  255.     publisher.publish(vel_msg)
  256.  
  257.     # Update plan every 1s.
  258.     time_since = current_time - previous_time
  259.     if current_path and time_since < 2.:
  260.       rate_limiter.sleep()
  261.       continue
  262.     previous_time = current_time
  263.  
  264.     # Run RRT.
  265.     start_node, final_node = rrt.rrt(slam.pose, goal.position, slam.occupancy_grid)
  266.     current_path = get_path(final_node)
  267.     if not current_path:
  268.       print('Unable to reach goal position:', goal.position)
  269.    
  270.     # Publish path to RViz.
  271.     path_msg = Path()
  272.     path_msg.header.seq = frame_id
  273.     path_msg.header.stamp = rospy.Time.now()
  274.     path_msg.header.frame_id = 'map'
  275.     for u in current_path:
  276.       pose_msg = PoseStamped()
  277.       pose_msg.header.seq = frame_id
  278.       pose_msg.header.stamp = path_msg.header.stamp
  279.       pose_msg.header.frame_id = 'map'
  280.       pose_msg.pose.position.x = u[X]
  281.       pose_msg.pose.position.y = u[Y]
  282.       path_msg.poses.append(pose_msg)
  283.     path_publisher.publish(path_msg)
  284.  
  285.     rate_limiter.sleep()
  286.     frame_id += 1
  287.  
  288.  
  289. if __name__ == '__main__':
  290.   parser = argparse.ArgumentParser(description='Runs RRT navigation')
  291.   args, unknown = parser.parse_known_args()
  292.   try:
  293.     run(args)
  294.   except rospy.ROSInterruptException:
  295.     pass
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top