def rrt_target(self, target):
start_node, final_node = rrt.rrt(self.pose(), target,
self.original_grid)
if final_node is not None:
self.path = rrt.get_path(final_node)
return final_node is not None
def load_goals(self):
'''
load_goals loads the goal (or goal list for the option al part) into
the x y theta variables.
'''
filepath = rospy.get_param('file',0)
x_goal = rospy.get_param('x', self.x_start)
y_goal = rospy.get_param('y', self.y_start)
grid_map = np.array(imread(filepath))
grid_map = grid_map[:,:,0]
q_start = [self.x_start, self.y_start] #[200,200]
q_goal = [x_goal, y_goal] #[200,350]
print q_start
print q_goal
k = 10000
delta_q = 10
p = 0.3
path = rrt(grid_map,q_start,q_goal,k,delta_q,p)
print 'Path: ',path
n = path.shape[0]
self.x = (path[:,1]-self.offset_y)*self.scale
self.y = (path[:,0]-self.offset_x)*self.scale
self.theta = 0*self.x
for i in np.arange(0,n-1):
x1 = self.x[i]
y1 = self.y[i]
x2 = self.x[i+1]
y2 = self.y[i+1]
self.theta[i] = self.angle_wrap(np.arctan2(y2-y1,x2-x1))
self.trajectory = np.vstack((self.trajectory, np.array([self.x[i], self.y[i], self.x[i+1], self.y[i+1]])));
self.theta[n-1] = self.theta[n-2]
print 'Trajectory: ', self.trajectory
self.num_goals = self.x.size
self.params_loaded = True
self.active_goal = 1
self.print_goals()
def main():
drawing.screen = pg.display.set_mode((WIDTH, HEIGHT))
gameState = 'waiting'
while True:
event = pg.event.poll()
mousePos = pg.mouse.get_pos()
gameState = events.mainHandler(event, gameState, mousePos)
if gameState == 'quit':
return
elif gameState == 'start-positioning':
drawing.startPos = mousePos
elif gameState == 'goal-positioning':
drawing.goalPos = mousePos
elif gameState == 'drawing':
drawing.drawObstacle(mousePos)
elif gameState == 'erasing':
drawing.eraseObstacle(mousePos)
elif gameState == 'clear':
drawing.clearObstacles()
elif gameState == 'save':
drawing.saveObstacles()
elif gameState == 'load':
drawing.loadObstacles()
elif gameState == 'rrt':
drawing.clearEdgesPool()
tree = rrt(drawing.startPos, drawing.goalPos,
drawing.obstaclesSurface)
if tree: # A path was found:
drawing.drawPath(tree)
gameState = 'path-found'
else: # User terminated the algorithm's execution:
gameState = 'waiting'
drawing.update()
def rrt_planner(self, laser_scan, max_iterations):
pos = self.pos
yaw = self.yaw
# convert laser_scan to points
points = []
a_min = laser_scan.angle_min
a_max = laser_scan.angle_max
a_inc = laser_scan.angle_increment
r_min = laser_scan.range_min
r_max = laser_scan.range_max
ranges = laser_scan.ranges
angle = a_min
for r in ranges:
if r <= r_max and r >= r_min and angle < a_max:
x = np.cos(angle + yaw) * r + pos[0]
y = np.sin(angle + yaw) * r + pos[1]
radius = 0.05
points.append([x, y, radius])
angle += a_inc
max_iterations = max_iterations
X_start = rrt.SE2_from_param([yaw, pos[0], pos[1]])
X_goal = self.X_goal
vehicle_radius = 0.15
box = [-12, 12, -12, 12]
ret = rrt.rrt(X_start=X_start,
X_goal=X_goal,
vehicle_radius=vehicle_radius,
box=box,
collision_points=points,
plot=True,
max_iterations=max_iterations,
dist_plan=1,
tolerance=0.15)
return ret
def run(args):
rospy.init_node('rrt_navigation')
# Update control every 100 ms.
rate_limiter = rospy.Rate(100)
publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
path_publisher = rospy.Publisher('/path', Path, queue_size=1)
slam = SLAM()
goal = GoalPose()
frame_id = 0
current_path = []
previous_time = rospy.Time.now().to_sec()
# Stop moving message.
stop_msg = Twist()
stop_msg.linear.x = 0.
stop_msg.angular.z = 0.
# Make sure the robot is stopped.
i = 0
while i < 10 and not rospy.is_shutdown():
publisher.publish(stop_msg)
rate_limiter.sleep()
i += 1
while not rospy.is_shutdown():
slam.update()
current_time = rospy.Time.now().to_sec()
# Make sure all measurements are ready.
# Get map and current position through SLAM:
# > roslaunch exercises slam.launch
if not goal.ready or not slam.ready:
rate_limiter.sleep()
continue
goal_reached = np.linalg.norm(slam.pose[:2] - goal.position) < .2
if goal_reached:
print("Goal Reached")
publisher.publish(stop_msg)
rate_limiter.sleep()
continue
# Follow path using feedback linearization.
position = np.array([
slam.pose[X] + EPSILON * np.cos(slam.pose[YAW]),
slam.pose[Y] + EPSILON * np.sin(slam.pose[YAW])
],
dtype=np.float32)
v = get_velocity(position, np.array(current_path, dtype=np.float32))
u, w = feedback_linearized(slam.pose, v, epsilon=EPSILON)
vel_msg = Twist()
vel_msg.linear.x = u
vel_msg.angular.z = w
publisher.publish(vel_msg)
# Update plan every 1s.
time_since = current_time - previous_time
if current_path and time_since < 2.:
rate_limiter.sleep()
continue
previous_time = current_time
# Run RRT.
start_node, final_node = rrt.rrt(slam.pose, goal.position,
slam.occupancy_grid)
current_path = get_path(final_node)
if not current_path:
print('Unable to reach goal position:', goal.position)
# Publish path to RViz.
path_msg = Path()
path_msg.header.seq = frame_id
path_msg.header.stamp = rospy.Time.now()
path_msg.header.frame_id = 'map'
for u in current_path:
pose_msg = PoseStamped()
pose_msg.header.seq = frame_id
pose_msg.header.stamp = path_msg.header.stamp
pose_msg.header.frame_id = 'map'
pose_msg.pose.position.x = u[X]
pose_msg.pose.position.y = u[Y]
path_msg.poses.append(pose_msg)
path_publisher.publish(path_msg)
rate_limiter.sleep()
frame_id += 1
from rrt import rrt
XDIM = 640
YDIM = 480
w = 100
w2 = 25
h = 150
h2 = 60
X0 = XDIM / 5
Y0 = YDIM / 3
X1 = 4 * XDIM / 5
Y1 = 2 * YDIM / 3
Obs = {}
Obs[0] = [(X0 + w2, Y0 + h2), (X0, Y0 + h2 / 2), (X0, Y0), (X0 + w, Y0),
(X0 + w, Y0 + h), (X0, Y0 + h), (X0, Y0 + h - h2 / 2),
(X0 + w2, Y0 + h - h2)]
XY_START = (X0 + w / 2, Y0 + 3 * h / 4) # Start in the trap
XY_GOAL = (4 * XDIM / 5, 5 * YDIM / 6)
game = rrt(Obs, XY_START, XY_GOAL, XDIM, YDIM)
game.runGame()
start = np.array([0, 0, 0, 0, 0, 0])
goal = np.array([-1.3, 0, 0.25, 0, 0.5, 0])
# Run Astar code
# path = Astar(deepcopy(map_struct), deepcopy(start), deepcopy(goal))
# IMPORTANT HYPERPARAMETERS FOR RRT*FN ****************************
max_nodes = 5000
max_iter = 3000
# or run rrt code
path, cost = rrt(deepcopy(map_struct),
deepcopy(start),
deepcopy(goal),
max_nodes,
max_iter,
stepsize=0.1,
neighbour_radius=0.15,
bias_ratio=5,
bias_radius=0.075,
optimize=True)
# start ROS
lynx = ArmController()
sleep(1) # wait for setup
collision = False
# Take out
lynx.set_pos(start)
sleep(5)
# iterate over target waypoints
if __name__ == '__main__':
# Update map location with the location of the target map
map_struct = loadmap("maps/map4.txt")
start = np.array([0, 0, 0, 0, 0, 0])
#goal = np.array([0, -0.3, -0.3, 0, 0, 0])
goal = np.array([-1.3, 0.1, 0.5, 0, 0, 0])
#Start timing
start_time = timeit.default_timer()
# Run Astar code
#path = Astar(deepcopy(map_struct), deepcopy(start), deepcopy(goal))
# or run rrt code
#path = rrt(deepcopy(map_struct), deepcopy(goal), deepcopy(start))
path = rrt(deepcopy(map_struct), deepcopy(start), deepcopy(goal))
#End Timing
elapsed = timeit.default_timer() - start_time
print("Time of Execution: ", elapsed)
# start ROS
lynx = ArmController()
sleep(1) # wait for setup
collision = False
# iterate over target waypoints
for q in path:
print("Goal:")
print(q)
def ogrid_callback(self, msg):
self.flag = 1;
self.counter = self.counter + 1
start = self.rostime()
self.ogrid = np.array(msg.data).reshape((msg.info.height, msg.info.width))
print("Shape ogrid", self.ogrid.shape)
#print("Ogrid patch", self.ogrid[0:50, 0:50])
self.ogrid[self.ogrid < 0] = 50
#print("Ogrid patch2" self.ogrid[100:150,100:150])
#print("MAX OGRID", np.max(self.ogrid))
#print("MIN OGRID", np.min(self.ogrid))
RRT = rrt.rrt(self.ogrid)
RRT.startPoint = np.array([self.x_pos, self.y_pos])
RRT.goalPoint = np.array([self.x_goal, self.y_goal])
RRT.build_tree()
print("START POINT", RRT.startPoint)
print("GOAL POINT", RRT.goalPoint)
# GET PATH FOR PLOTTING
path = RRT.get_path()
print("PATH ", path)
path_shape = path.shape
path = path.reshape(path_shape[0]/2,2)
print("PATH1", path)
path = np.flip(path, axis = 0)
print("PATH2", path)
path = path*0.05
print("Path shape", path_shape[0])
#self.path = path
#self.path_shape = path_shape
for i in range(path_shape[0]/2) :
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE
#marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.a = 1.0
marker.color.r = 1.0 # make it red
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = path[i, 0]
marker.pose.position.y = path[i, 1]
marker.pose.position.z = 0.0
self.markerArray.markers.append(marker)
# make the PoseArray
#self.poseArray.header.stamp = self.rostime()
self.poseArray.header.frame_id = "/map"
somePose = Pose()
#somePose.header.frame_id = "/map" ###
somePose.position.x = path[i, 0]
somePose.position.y = path[i, 1]
somePose.position.z = 0.0
try :
#if (i < path_shape[0]/2 -1) :
#print("try")
#ang_i_cur = math.atan2(path[i, 0], path[i, 1])
#ang_i_next = math.atan2(path[i+1, 0], path[i+1, 1])
ang_i_cur = math.atan2(path[i, 1], path[i, 0])
ang_i_next = math.atan2(path[i+1, 1], path[i+1, 0])
#angle_to_next = (ang_i_next + ang_i_cur + math.pi) % (2.0*math.pi) - math.pi
angle_to_next = math.atan2(path[i+1, 1]-path[i, 1], path[i+1, 0]-path[i, 0])
self.prev_theta = angle_to_next
print("ang_i_cur", ang_i_cur)
print("ang_i_next", ang_i_next)
print("Angle to next", ang_i_next)
#(roll, pitch, theta_pos) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
(somePose.orientation.x, somePose.orientation.y, somePose.orientation.z, somePose.orientation.w) = \
quaternion_from_euler(0.0, 0.0, angle_to_next)
except :
print("except")
somePose.orientation.x = 0.0
somePose.orientation.y = 0.0
somePose.orientation.z = 0.0
somePose.orientation.w = 1.0
self.poseArray.poses.append(somePose)
# GET POINTS FOR PLOTTING
all_points = RRT.get_all_nodes()
all_points_shape = all_points.shape
print("All points shape", all_points_shape)
#all_points = all_points.reshape(all_points_shape[0]/2,2)
all_points = all_points*0.05
#print("PATH", all_points)
#self.path = path
#self.path_shape = path_shape
for i in range(all_points_shape[0]/2) :
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE
#marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.r = 0.0 # make it red
marker.color.g = 0.0
marker.color.b = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = all_points[i, 0]
marker.pose.position.y = all_points[i, 1]
marker.pose.position.z = 0
self.markerArray.markers.append(marker)
id = 0
for m in self.markerArray.markers:
m.id = id
id += 1
self.publisher.publish(self.markerArray)
self.markerArray = MarkerArray()
path_shape = path.shape
if path_shape[0] > 4 :
self.pose_array_pub.publish(self.poseArray)
self.poseArray = PoseArray()
self.ogrid_origin = np.array([msg.info.origin.position.x, msg.info.origin.position.y])
self.ogrid_cpm = 1 / msg.info.resolution
try:
print("TRY")
except:
print("\n(WARNING: something went wrong in reevaluate_plan)\n")
elapsed = abs(self.rostime() - start)
if elapsed > 1:
print("\n(WARNING: ogrid callback is taking {} seconds)\n".format(np.round(elapsed, 2)))
from planar_trajectory import * from planarsim import * from rrt import rrt #in this file there are different simple tests. You may have to run each one a couple of times #because the probabilistic, random nature means that goal location won't always be found #very simple initial test start = (0, 0, 0) goal = [3, 3, 0] obstacles = [[4, 4, 1, 6]] test_rrt = rrt(start, 3, obstacles, 2) test_rrt.rrt_complete(20, goal) # #middle difficulty test # #this is more challenging because the necessary distance between point and goal is smaller # #in addition there are more obstacles # start = (0, 0, 0) # goal = [-3, -3, 0] # obstacles = [[-6, -6, 7, 1], [4, 4, 2, 2]] # test_rrt = rrt(start, 3, obstacles, 1) # test_rrt.rrt_complete(100, goal) # #much more complex test # #the start and goal are much further apart and there are more obstacles in the way # start = (-5, -5, 0) # goal = [5, 5, 0] # obstacles = [[-6, -6, 7, 1], [-6, 6, 7, 1]]
#theta* result, saved for later
nearest = None
path = [(280, 0), (73, 38), (72, 39), (33, 130), (15, 190), (8, 280), None]
for first, second, third in zip(path, path[1:], path[2:]):
angle1 = rrt.anglebetween([1, 0], np.subtract(second, first))
if nearest is not None:
angle1 = nearest[1]
first = nearest[0]
if third == None:
angle2 = angle1
else:
angle2 = rrt.anglebetween([1, 0], np.subtract(third, second))
begin = (first, rrt.standardangle(angle1))
end = (second, rrt.standardangle(angle2))
solution, graph, camefrom = rrt.rrt(begin, end, debug=True)
if solution is not None:
rrt.drawpath(solution, camefrom)
else:
print("Didn't find solution")
# The tree is only useful to visualize if small (limit K)
#rrt.drawtree(begin,graph,camefrom)
# find nearest on graph to solution? pick that???
nearest, mindist = rrt.findnearest(graph, end)
rrt.drawpath(nearest, camefrom)
rrt.draw_bicycle(nearest[0], nearest[1], 0, color='darkgray')
rrt.draw_bicycle(begin[0], begin[1], 0, color='red')
rrt.draw_bicycle(end[0], end[1], 0, color='lime')
def run():
rospy.init_node('obstacle_avoidance')
# Update control every 50 ms.
rate_limiter = rospy.Rate(50)
publishers = [None] * len(ROBOT_NAMES)
lasers = [None] * len(ROBOT_NAMES)
groundtruths = [None] * len(ROBOT_NAMES)
# RRT path
# If RUN_RRT is False, load the predefined path
if not RUN_RRT:
current_path = MAP_PARAMS["RRT_PATH"]
else:
current_path = None
vel_msgs = [None] * len(ROBOT_NAMES)
for i, name in enumerate(ROBOT_NAMES):
publishers[i] = rospy.Publisher('/' + name + '/cmd_vel',
Twist,
queue_size=5)
lasers[i] = SimpleLaser(name)
groundtruths[i] = GroundtruthPose(name)
# Load map. (in here so it is only computed once)
with open(
os.path.expanduser(
'~/catkin_ws/src/exercises/project/python/{}.yaml'.format(
MAP))) as fp:
data = yaml.load(fp)
img = rrt.read_pgm(
os.path.expanduser(
'~/catkin_ws/src/exercises/project/python/{}.pgm'.format(MAP)),
data['image'])
occupancy_grid = np.empty_like(img, dtype=np.int8)
occupancy_grid[:] = UNKNOWN
occupancy_grid[img < .1] = OCCUPIED
occupancy_grid[img > .9] = FREE
# Transpose (undo ROS processing).
occupancy_grid = occupancy_grid.T
# Invert Y-axis.
occupancy_grid = occupancy_grid[:, ::-1]
occupancy_grid = rrt.OccupancyGrid(occupancy_grid, data['origin'],
data['resolution'])
while not rospy.is_shutdown():
# Make sure all measurements are ready.
if not all(laser.ready for laser in lasers) or not all(
groundtruth.ready for groundtruth in groundtruths):
rate_limiter.sleep()
continue
# Compute RRT for the leader only
while not current_path:
start_node, final_node = rrt.rrt(groundtruths[LEADER_ID].pose,
GOAL_POSITION, occupancy_grid)
current_path = rrt_navigation.get_path(final_node)
# print("CURRENT PATH: ", current_path)
# get the RRT velocity for the leader robot
position = np.array([
groundtruths[LEADER_ID].pose[X] +
EPSILON * np.cos(groundtruths[LEADER_ID].pose[YAW]),
groundtruths[LEADER_ID].pose[Y] +
EPSILON * np.sin(groundtruths[LEADER_ID].pose[YAW])
],
dtype=np.float32)
rrt_velocity = rrt_navigation.get_velocity(
position, np.array(current_path, dtype=np.float32))
# get robot poses
robot_poses = np.array(
[groundtruths[i].pose for i in range(len(groundtruths))])
# get the velocities for all the robots
us, ws, desired_positions = gcv.get_combined_velocities(
robot_poses=robot_poses,
leader_rrt_velocity=rrt_velocity,
lasers=lasers)
save_errors(robot_poses, desired_positions)
for i in range(len(us)):
vel_msgs[i] = Twist()
vel_msgs[i].linear.x = us[i]
vel_msgs[i].angular.z = ws[i]
for i, _ in enumerate(ROBOT_NAMES):
publishers[i].publish(vel_msgs[i])
rate_limiter.sleep()
def newGoal(self, msg):
print("newGoal Callback")
self.x_goal = round(20 * msg.pose.position.x)
self.y_goal = round(20 * msg.pose.position.y)
rot_q = msg.pose.orientation
(roll_goal, pitch_goal, theta_goal) = euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w])
#theta = yaw;
RRT = rrt.rrt(self.ogrid)
RRT.startPoint = np.array([self.x_pos, self.y_pos])
RRT.goalPoint = np.array([self.x_goal, self.y_goal])
RRT.build_tree()
print("START POINT", RRT.startPoint)
print("GOAL POINT", RRT.goalPoint)
# GET PATH FOR PLOTTING
path = RRT.get_path()
print("PATH ", path)
path_shape = path.shape
path = path.reshape(path_shape[0] / 2, 2)
print("PATH1", path)
path = np.flip(path, axis=0)
print("PATH2", path)
path = path * 0.05
print("Path shape", path_shape[0])
#self.path = path
#self.path_shape = path_shape
for i in range(path_shape[0] / 2):
"""
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE
#marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.a = 1.0
marker.color.r = 1.0 # make it red
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = path[i, 0]
marker.pose.position.y = path[i, 1]
marker.pose.position.z = 0.0
self.markerArray.markers.append(marker)
"""
# make the PoseArray
#self.poseArray.header.stamp = self.rostime()
self.poseArray.header.frame_id = "/map"
somePose = Pose()
#somePose.header.frame_id = "/map" ###
somePose.position.x = path[i, 0]
somePose.position.y = path[i, 1]
somePose.position.z = 0.0
try:
#if (i < path_shape[0]/2 -1) :
#print("try")
#ang_i_cur = math.atan2(path[i, 0], path[i, 1])
#ang_i_next = math.atan2(path[i+1, 0], path[i+1, 1])
ang_i_cur = math.atan2(path[i, 1], path[i, 0])
ang_i_next = math.atan2(path[i + 1, 1], path[i + 1, 0])
#angle_to_next = (ang_i_next + ang_i_cur + math.pi) % (2.0*math.pi) - math.pi
angle_to_next = math.atan2(path[i + 1, 1] - path[i, 1],
path[i + 1, 0] - path[i, 0])
self.prev_theta = angle_to_next
print("ang_i_cur", ang_i_cur)
print("ang_i_next", ang_i_next)
print("Angle to next", ang_i_next)
#(roll, pitch, theta_pos) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
(somePose.orientation.x, somePose.orientation.y, somePose.orientation.z, somePose.orientation.w) = \
quaternion_from_euler(0.0, 0.0, angle_to_next)
except:
print("except")
somePose.orientation.x = 0.0
somePose.orientation.y = 0.0
somePose.orientation.z = 0.0
somePose.orientation.w = 1.0
self.poseArray.poses.append(somePose)
# GET POINTS FOR PLOTTING
all_points = RRT.get_all_nodes()
all_points_shape = all_points.shape
print("All points shape", all_points_shape)
#all_points = all_points.reshape(all_points_shape[0]/2,2)
all_points = all_points * 0.05
#print("PATH", all_points)
#self.path = path
#self.path_shape = path_shape
for i in range(all_points_shape[0] / 2):
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE
#marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.r = 0.0 # make it red
marker.color.g = 0.0
marker.color.b = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = all_points[i, 0]
marker.pose.position.y = all_points[i, 1]
marker.pose.position.z = 0
self.markerArray.markers.append(marker)
id = 0
for m in self.markerArray.markers:
m.id = id
id += 1
self.publisher.publish(self.markerArray)
self.markerArray = MarkerArray()
path_shape = path.shape
if path_shape[0] > 4:
self.pose_array_pub.publish(self.poseArray)
self.poseArray = PoseArray()
def run():
rospy.init_node('obstacle_avoidance')
# Update control every 50 ms.
rate_limiter = rospy.Rate(50)
publisher = rospy.Publisher('/' + ROBOT_NAME + '/cmd_vel', Twist, queue_size=5)
laser = SimpleLaser(ROBOT_NAME)
groundtruth = GroundtruthPose(ROBOT_NAME)
vel_msg = None
# RRT path
# If RUN_RRT is False, load the predefined path
if not RUN_RRT:
current_path = MAP_PARAMS["RRT_PATH"]
else:
current_path = None
# Load map. (in here so it is only computed once)
with open(os.path.expanduser('~/catkin_ws/src/exercises/project/python/{}.yaml'.format(MAP))) as fp:
data = yaml.load(fp)
img = rrt.read_pgm(os.path.expanduser('~/catkin_ws/src/exercises/project/python/{}.pgm'.format(MAP)), data['image'])
occupancy_grid = np.empty_like(img, dtype=np.int8)
occupancy_grid[:] = UNKNOWN
occupancy_grid[img < .1] = OCCUPIED
occupancy_grid[img > .9] = FREE
# Transpose (undo ROS processing).
occupancy_grid = occupancy_grid.T
# Invert Y-axis.
occupancy_grid = occupancy_grid[:, ::-1]
occupancy_grid = rrt.OccupancyGrid(occupancy_grid, data['origin'], data['resolution'])
while not rospy.is_shutdown():
# Make sure all measurements are ready.
if not laser.ready or not groundtruth.ready:
rate_limiter.sleep()
continue
LEADER_POSE = groundtruth.leader_pose
# Compute RRT on the leader only
if ROBOT_ID == LEADER_ID:
while not current_path:
start_node, final_node = rrt.rrt(LEADER_POSE, GOAL_POSITION, occupancy_grid)
current_path = rrt_navigation.get_path(final_node)
# print("CURRENT PATH: ", current_path)
# get the RRT velocity for the leader robot
position = np.array([LEADER_POSE[X] + EPSILON*np.cos(LEADER_POSE[YAW]),
LEADER_POSE[Y] + EPSILON*np.sin(LEADER_POSE[YAW])], dtype=np.float32)
LEADER_VELOCITY = rrt_navigation.get_velocity(position, np.array(current_path, dtype=np.float32))
else:
# Let the leader velocity be 0, since the leader pose will update and the
# formation velocity will correctly move the robot
LEADER_VELOCITY = np.array([0., 0.])
# get the velocity for this robot
u, w, desired_position = gcv.get_combined_velocity(groundtruth.pose, LEADER_POSE, LEADER_VELOCITY, laser, ROBOT_ID)
save_error(groundtruth.pose[:2], desired_position)
vel_msg = Twist()
vel_msg.linear.x = u
vel_msg.angular.z = w
publisher.publish(vel_msg)
rate_limiter.sleep()
def run(args):
rospy.init_node('rrt_navigation')
exploration_start = rospy.Time.now().to_sec()
# Update control every 100 ms.
rate_limiter = rospy.Rate(100)
publisher = rospy.Publisher(ROBOT_NS + '/cmd_vel', Twist, queue_size=5)
path_publisher = rospy.Publisher(ROBOT_NS + '/path', Path, queue_size=1)
slam = SLAM()
goal = Explortaion(slam)
frame_id = 0
current_path = []
previous_time = rospy.Time.now().to_sec()
# Stop moving message.
stop_msg = Twist()
stop_msg.linear.x = 0.
stop_msg.angular.z = 0.
# Make sure the robot is stopped.
i = 0
while i < 10 and not rospy.is_shutdown():
publisher.publish(stop_msg)
rate_limiter.sleep()
i += 1
while not rospy.is_shutdown():
if goal.success:
print(rospy.Time.now().to_sec() - exploration_start,
's of exploration. finished! robot ', ROBOT_NS)
i = 0
while i < 10 and not rospy.is_shutdown():
publisher.publish(stop_msg)
rate_limiter.sleep()
i += 1
# for debug
print(
'the grid ', slam.occupancy_grid.values.shape, ' length:',
float(slam.occupancy_grid.values.shape[0]) *
slam.occupancy_grid.resolution)
print(slam.occupancy_grid.values)
break
continue
slam.update()
current_time = rospy.Time.now().to_sec()
# Make sure all measurements are ready.
# Get map and current position through SLAM:
# > roslaunch exercises slam.launch
if not slam.ready or not goal.ready:
rate_limiter.sleep()
continue
# now we need to get the goal
# if the goal is ready but not reached
if not goal.current_goal_reached:
# stick to the original goal
print('now for goal ', goal.position[0], ' ', goal.position[1])
# but remember to clear the RRT path calculated inside the exploration class
goal.rrt_final_node = None
# else start a new goal
else:
# Make sure the robot is stopped.
i = 0
while i < 10 and not rospy.is_shutdown():
publisher.publish(stop_msg)
rate_limiter.sleep()
i += 1
print('old goal reached, now for new goal')
goal.rrt_final_node = None
goal.get_next_dest_for_local()
continue
# goal_reached = goal.current_goal_reached
# if goal_reached:
# publisher.publish(stop_msg)
# rate_limiter.sleep()
# continue
# Follow path using feedback linearization.
position = np.array([
slam.pose[X] + EPSILON * np.cos(slam.pose[YAW]),
slam.pose[Y] + EPSILON * np.sin(slam.pose[YAW])
],
dtype=np.float32)
v = get_velocity(position, np.array(current_path, dtype=np.float32))
u, w = feedback_linearized(slam.pose, v, epsilon=EPSILON)
vel_msg = Twist()
vel_msg.linear.x = u
vel_msg.angular.z = w
publisher.publish(vel_msg)
# Update plan every 1s.
time_since = current_time - previous_time
if current_path and time_since < 2.:
rate_limiter.sleep()
continue
previous_time = current_time
# Run RRT.
# if we have got an RRT path from the class, return it
if goal.rrt_final_node is None:
start_node, final_node = rrt.rrt(slam.pose, goal.position,
slam.occupancy_grid)
else:
final_node = goal.rrt_final_node
current_path = get_path(final_node)
if not current_path:
print('Unable to reach goal position:', goal.position,
' now change goal')
# Make sure the robot is stopped.
i = 0
while i < 10 and not rospy.is_shutdown():
publisher.publish(stop_msg)
rate_limiter.sleep()
i += 1
goal._next_dest = None
goal.rrt_final_node = None
# goal.get_next_dest_for_local()
# Publish path to RViz.
path_msg = Path()
path_msg.header.seq = frame_id
path_msg.header.stamp = rospy.Time.now()
path_msg.header.frame_id = 'map'
for u in current_path:
pose_msg = PoseStamped()
pose_msg.header.seq = frame_id
pose_msg.header.stamp = path_msg.header.stamp
pose_msg.header.frame_id = 'map'
pose_msg.pose.position.x = u[X]
pose_msg.pose.position.y = u[Y]
path_msg.poses.append(pose_msg)
path_publisher.publish(path_msg)
rate_limiter.sleep()
frame_id += 1
