def callback(self, msg): values = np.array(msg.data, dtype=np.int8).reshape((msg.info.width, msg.info.height)) processed = np.empty_like(values) processed[:] = rrt.FREE processed[values < 0] = rrt.UNKNOWN processed[values > 50] = rrt.OCCUPIED processed = processed.T origin = [msg.info.origin.position.x, msg.info.origin.position.y, 0.] resolution = msg.info.resolution self._occupancy_grid = rrt.OccupancyGrid(processed, origin, resolution)
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 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()
