import matplotlib.image as mpimg
from grid_map import GridMap
from robot_model import RobotModel
dr = mpimg.imread('/home/kai/Pictures/dr.png')
global_grid_map = GridMap(dr, 1.0)
global_grid_map.print()
robot_model = RobotModel(2.0)
controls = robot_model.get_controls((639, 200), global_grid_map)
print(controls)
rospy.init_node('test_a_star_planner', anonymous=True)
vehicle_config = yaml.load(
open('/home/kai/catkin_ws/src/drproj/vehicle_config.yaml'))
half_length = 0.5 * vehicle_config['length']
half_width = 0.5 * vehicle_config['width']
radius = sqrt(half_length**2 + half_width**2)
robot_model = RobotModel(radius)
global_planner = AStarPlanner(robot_model)
free = mpimg.imread('/home/kai/catkin_ws/src/drproj/free.png')
global_map_config = yaml.load(open('/home/kai/catkin_ws/src/drproj/free.yaml'))
global_map = GridMap(free, global_map_config['resolution'])
global_map.show('rviz_global_grid_map')
global_map.print()
global_mission = yaml.load(
open('/home/kai/catkin_ws/src/drproj/global_mission.yaml'))
start_grid_x = int(global_mission['start'][0] / global_map.resolution + 0.5)
start_grid_y = int(global_mission['start'][1] / global_map.resolution + 0.5)
goal_grid_x = int(global_mission['goal'][0] / global_map.resolution + 0.5)
goal_grid_y = int(global_mission['goal'][1] / global_map.resolution + 0.5)
start = (start_grid_x, start_grid_y)
goal = (goal_grid_x, goal_grid_y)
path = global_planner.get_path(start, goal, global_map)
global_planner.show_path('rviz_global_path', global_map)
print(path)
global_planner.send_path('global_path')
class MovingObs(object):
def __init__(self,
config_file,
map_config_file,
map_file,
is_clear=False,
is_pub_id=True,
resolution=0.1):
# Member
f = open(config_file)
self.config = yaml.load(f)
f = open(map_config_file)
self.map_config = yaml.load(f)
map_array = mpimg.imread(map_file)
self.grid_map = GridMap(map_array, self.map_config['resolution'])
self.grid_map.print()
self.resolution = resolution # m
self.trajs = []
self.last_id = None
self.cur_id = 0
self.is_pub_id = is_pub_id
# Refine
self.refine()
# Clear grid map.
if is_clear:
self.clear_map()
# Spawn obstacles into the map.
for traj in self.trajs:
x = int(traj[self.cur_id][0] / self.grid_map.resolution + 0.5)
y = int(traj[self.cur_id][1] / self.grid_map.resolution + 0.5)
self.grid_map.set_grid(x, y, 1.0)
self.last_id = self.cur_id
self.cur_id += 1
def interpolate_two_pts(self, start, end):
'''
Arguments
---------
start (A 'point' represented by list, meter)
end (A 'point' represented by list, meter)
Returns
-------
_ (A list of 'point', including 'start' but excluding 'end')
'''
dx = end[0] - start[0]
dy = end[1] - start[1]
if abs(dx) + abs(dy) == 0:
return []
x_resolution = self.resolution * dx / sqrt(dx**2 + dy**2)
y_resolution = self.resolution * dy / sqrt(dx**2 + dy**2)
max_dis = np.linalg.norm(np.subtract(end, start))
pts = []
pt = start.copy()
while np.linalg.norm(np.subtract(pt, start)) < max_dis:
pts.append(pt.copy())
pt[0] += x_resolution
pt[1] += y_resolution
return pts
def interpolate_multi_pts(self, multi_pts):
pts = []
for i in range(len(multi_pts) - 1):
start = multi_pts[i]
end = multi_pts[i + 1]
pts += self.interpolate_two_pts(start, end)
return pts
def refine(self):
self.trajs = []
if self.config is None:
return
for key in self.config:
self.trajs.append(self.interpolate_multi_pts(self.config[key]))
def run_once(self):
if self.is_pub_id:
pub = rospy.Publisher('cur_moving_obs_id',
Int64,
queue_size=10,
latch=True)
pub.publish(Int64(data=self.cur_id))
for traj in self.trajs:
i = min(self.last_id, len(traj) - 1)
x = int(traj[i][0] / self.grid_map.resolution + 0.5)
y = int(traj[i][1] / self.grid_map.resolution + 0.5)
self.grid_map.set_grid(x, y, 0.0)
i = min(self.cur_id, len(traj) - 1)
x = int(traj[i][0] / self.grid_map.resolution + 0.5)
y = int(traj[i][1] / self.grid_map.resolution + 0.5)
self.grid_map.set_grid(x, y, 1.0)
self.last_id = self.cur_id
self.cur_id += 1
def clear_map(self):
for x in range(self.grid_map.max_x):
for y in range(self.grid_map.max_y):
self.grid_map.set_grid(x, y, 0.0)
