def neighbors(self, state):
max_angle = utils.max_angle(self.min_turn_radius, state.radius)
self.thetas = np.linspace(-max_angle,
max_angle,
num=self.branch_factor)
self.euclidean_neighbors[:, 0] = state.radius * np.cos(
self.thetas + state.angle) + state.center[0]
self.euclidean_neighbors[:, 1] = state.radius * np.sin(
self.thetas + state.angle) + state.center[1]
# we use this several times, so compute this icky math here to avoid unnecessary computation
euc = self.euclidean_neighbors.copy()
utils.world_to_map(euc, self.map.map_info)
euc = np.round(euc).astype(int)
radii = self.map.get_distances(euc,
coord_convert=False,
check_bounds=True)
mask = np.logical_and(
np.invert(self.map.get_explored(euc, coord_convert=False)),
self.map.get_permissible(euc, coord_convert=False))
neighbors = map(
lambda c, r, t: Circle(
center=c.copy(
), radius=r, angle=state.angle + t, deflection=t),
self.euclidean_neighbors[mask, :], radii[mask], self.thetas[mask])
return neighbors
def out_of_bounds_poses(self, poses):
t0 = time.time()
grid_poses = poses.clone()
dprint('World Poses: ', grid_poses)
# t1 = time.time()
#print('time for clone:', t1 - t0)
Utils.world_to_map(grid_poses, self.map_info)
# t2 = time.time()
# print('time for util:', t2 - t1)
dprint('Grid Poses: ', grid_poses)
grid_poses.round_() # It's important to round, not floor
# t3 = time.time()
# print('time for round:', t3 - t2)
#grid_poses = grid_poses[:, :2] # Gets rid of theta
grid_poses = grid_poses.type(torch.cuda.LongTensor)
#t4 = time.time()
#print('time for type:', t4 - t3)
#map_indices = grid_poses[:, 0] + grid_poses[:, 1]
occupancyValues = self.permissible_region_torch[grid_poses[:, 1],
grid_poses[:, 0]]
#t5 = time.time()
#print('time for occupancy:', t5 - t4)
return occupancyValues
def startCB(self, msg):
#rospy.loginfo('recieved start msg')
self.get_omap()
self.start = np.array([(msg.pose.pose.position.x,
msg.pose.pose.position.y, 0)])
Utils.world_to_map(self.start, self.map_info)
self.start = np.array((self.start[0][0], 1300 - self.start[0][1],
self.start[0][2])).astype(int)
def final_cost(self, pose, state):
# TODO make a better final_cost (running tally of bound check)
np_pose = pose.cpu().numpy()
Utils.world_to_map(np_pose, self.map_info)
np_pose = np.round(np_pose).astype(int)
bounds_check = 10000 * self.permissible_region[np_pose[:, 0],
np_pose[:, 1]]
return torch.from_numpy(bounds_check).type(self.dtype)
def init_pose_cb(self, msg):
# Get the pose of the car w.r.t the map in meters
rx_trans = np.array(
[msg.pose.pose.position.x, msg.pose.pose.position.y],
dtype=np.float)
rx_rot = utils.quaternion_to_angle(msg.pose.pose.orientation)
# Get the pose of the car w.r.t the map in pixels
if self.map_info is not None:
map_rx_pose = utils.world_to_map(
(rx_trans[0], rx_trans[1], rx_rot), self.map_info)
# Update the pose of the car if either bounds checking is not enabled,
# or bounds checking is enabled but the car is in-bounds
if (self.permissible_region is None
or self.permissible_region[int(map_rx_pose[1] + 0.5),
int(map_rx_pose[0] + 0.5)] == 1):
self.cur_odom_to_base_lock.acquire()
self.cur_map_to_odom_lock.acquire()
# Compute where the car is w.r.t the odometry frame
offset_in_map = rx_trans - self.cur_map_to_odom_trans
self.cur_odom_to_base_trans = np.zeros(2, dtype=np.float)
self.cur_odom_to_base_trans[0] = offset_in_map[0] * np.cos(
-self.cur_map_to_odom_rot) - offset_in_map[1] * np.sin(
-self.cur_map_to_odom_rot)
self.cur_odom_to_base_trans[1] = offset_in_map[0] * np.sin(
-self.cur_map_to_odom_rot) + offset_in_map[1] * np.cos(
-self.cur_map_to_odom_rot)
self.cur_odom_to_base_rot = rx_rot - self.cur_map_to_odom_rot
self.cur_map_to_odom_lock.release()
self.cur_odom_to_base_lock.release()
def out_of_bounds(self, pose):
grid_poses = pose.clone().view(1, 3)
dprint('World Poses: ', grid_poses)
Utils.world_to_map(grid_poses, self.map_info)
dprint('Grid Poses: ', grid_poses)
grid_poses.round_()
grid_x = int(round(grid_poses[0][0]))
grid_y = int(round(grid_poses[0][1]))
occupancyVal = self.permissible_region[grid_y, grid_x]
if occupancyVal == 1:
#occupancyVal = self.map_data[grid_x + grid_y * self.map_info.width)]
# if occupancyVal == -1 or occupancyVal == 100:
print('Pose is invalid!!!!')
return 0
else:
#print('Valid pose: ', pose)
print('Pose is valid!!!')
return 1
def _ray(x: float, y: float, angle: float,
gridmap: OccupancyGrid) -> List[Tuple[int, int]]:
start = x + np.cos(angle) * SENSOR.MIN_RANGE, y + np.sin(
angle) * SENSOR.MIN_RANGE
end = x + np.cos(angle) * SENSOR.MAX_RANGE, y + np.sin(
angle) * SENSOR.MAX_RANGE
start, end = world_to_map([start, end], gridmap)
ray = bresenham_line(start, end)
return ray
def fuse_laser_scan(self, laser_scan: LaserScan):
"""Fuse the laser scan data into the probabilistic occupancy grid map."""
assert isinstance(laser_scan, LaserScan)
# laser scan points in map coordinates
points_r = self._read_laser_data(laser_scan)
points_w = robot_to_world(points_r, self.odometry)
points = world_to_map(points_w, self.map)
# extend the map if needed
extended = self._extend_map(points)
if extended:
points = world_to_map(points_w, self.map)
# robot position in map coordinates
robot = world_to_map([self.position], self.map)[0]
# raytrace the points and update the map accordingly
free, occupied = self._raytrace(robot, points)
self._update_map(free, occupied)
def goalCB(self, msg):
#rospy.loginfo('recieved goal msg')
self.goal = np.array([(msg.pose.position.x, msg.pose.position.y, 0)])
Utils.world_to_map(self.goal, self.map_info)
self.goal = np.array((self.goal[0][0], 1300 - self.goal[0][1],
self.goal[0][2])).astype(int)
#rospy.loginfo(self.goal)
if self.environment[self.goal[1]][self.goal[0]] == 0:
#rospy.loginfo(self.goal)
if self.start != None and self.goal != None:
#rospy.loginfo('getting path')
path = np.array(
self.rrt(self.environment, self.start,
self.goal)).astype(float)
#rospy.loginfo(path)
Utils.map_to_world(path, self.map_info)
path = path.astype(int)
#rospy.loginfo('visualizing')
self.visualize(path, self.start, self.goal)
else:
self.goal = None
def neighbors(self, state):
''' This function determines the set of points along the perimeter of the given
circle state that are accessible according to ackermann geometry and steering
angle limitations. Then it samples the permissible theta space to generate neighbors.
TODO: computation of neighboring thetas is not accurate - it should actually be
the direction tangent to the path arc at the intersection with the circle radius.
For now this works, and is a strict underestimate of the reachable thetas
'''
max_angle = utils.max_angle(self.min_turn_radius, state.radius)
percentage = np.power(2.0 * self.branch_factor / np.pi, 0.9)
actual_branch_factor = 2 * int(max_angle * percentage / 2.0) + 1
thetas = np.linspace(-max_angle, max_angle, num=actual_branch_factor)
euclidean_neighbors = np.zeros((actual_branch_factor, 2))
euclidean_neighbors[:, 0] = state.radius * np.cos(
thetas + state.angle) + state.center[0]
euclidean_neighbors[:, 1] = state.radius * np.sin(
thetas + state.angle) + state.center[1]
# perform coordinate space conversion here and then index into the exploration and permissible
# buffers to prune states which are not permissible or already explored
euc = euclidean_neighbors.copy()
utils.world_to_map(euc, self.map.map_info)
euc = np.round(euc).astype(int)
radii = self.map.get_distances(
euc, coord_convert=False, check_bounds=True) - 0.05
radii = np.clip(radii, 0.0, self.max_circle_radius)
mask = np.logical_and(
np.invert(self.map.get_explored(euc, coord_convert=False)),
self.map.get_permissible(euc, coord_convert=False))
# generate a neighbor state for each of the permissible
neighbors = map(
lambda c, r, t: Circle(
center=c.copy(), radius=r, angle=state.angle + t, deflection=t
), euclidean_neighbors[mask, :], radii[mask], thetas[mask])
return neighbors
def running_cost(self, pose, goal, ctrl, noise):
np_pose = pose.cpu().numpy() # Kx3
Utils.world_to_map(np_pose, self.map_info)
np_pose = np.round(np_pose).astype(int)
np_pose[:, 0] = np.clip(np_pose[:, 0], 0, self.map_height - 1)
np_pose[:, 1] = np.clip(np_pose[:, 1], 0, self.map_width - 1)
bounds_check = 100 * self.permissible_region[
np_pose[:, 0], np_pose[:, 1]] # TODO check this bounds checks
bounds_check = torch.from_numpy(bounds_check).type(self.dtype)
#delta = pose[:,0:2] - torch.from_numpy(goal[0:2]).type(self.dtype)
delta = pose - torch.from_numpy(goal).type(self.dtype)
pose_cost = 2.5 * torch.sum(delta**2, 1)
#pose_cost = 2.5*torch.sum((pose[:,0:2]-torch.from_numpy(goal[0:2]).type(self.dtype))**2, 1)
ctrl_cost = torch.abs(
self._lambda *
((noise * (1 / self.sigma)).mm(ctrl.expand(1, 2).transpose(0, 1))))
#print("costs:",torch.mean(pose_cost), torch.mean(ctrl_cost), torch.mean(bounds_check))
return pose_cost + torch.squeeze(ctrl_cost) + bounds_check
def timer_cb(self, event):
now = rospy.Time.now()
# Publish a tf between map and odom if one does not already exist
# Otherwise, get the most recent tf between map and odom
self.cur_map_to_odom_lock.acquire()
try:
tmp_trans, tmp_rot = self.transformer.lookupTransform(
"/odom", "/map", rospy.Time(0))
self.cur_map_to_odom_trans[0] = tmp_trans[0]
self.cur_map_to_odom_trans[1] = tmp_trans[1]
self.cur_map_to_odom_rot = (
tf.transformations.euler_from_quaternion(tmp_rot))[2]
if tmp_trans[2] == -0.0001:
self.br.sendTransform(
(
self.cur_map_to_odom_trans[0],
self.cur_map_to_odom_trans[1],
0.0001,
),
tf.transformations.quaternion_from_euler(
0, 0, self.cur_map_to_odom_rot),
now,
"/odom",
"/map",
)
except Exception:
self.br.sendTransform(
(self.cur_map_to_odom_trans[0], self.cur_map_to_odom_trans[1],
0.0001),
tf.transformations.quaternion_from_euler(
0, 0, self.cur_map_to_odom_rot),
now,
"/odom",
"/map",
)
self.cur_map_to_odom_lock.release()
# Get the time since the last update
if self.last_stamp is None:
self.last_stamp = now
dt = (now - self.last_stamp).to_sec()
# Add noise to the speed
self.last_speed_lock.acquire()
v = self.last_speed + np.random.normal(
loc=self.SPEED_OFFSET * self.last_speed,
scale=self.SPEED_NOISE,
size=1)
self.last_speed_lock.release()
# Add noise to the steering angle
self.last_steering_angle_lock.acquire()
delta = self.last_steering_angle + np.random.normal(
loc=self.STEERING_ANGLE_OFFSET * self.last_steering_angle,
scale=self.STEERING_ANGLE_NOISE,
size=1,
)
self.last_steering_angle_lock.release()
self.cur_odom_to_base_lock.acquire()
# Apply kinematic car model to the previous pose
new_pose = np.array(
[
self.cur_odom_to_base_trans[0],
self.cur_odom_to_base_trans[1],
self.cur_odom_to_base_rot,
],
dtype=np.float,
)
if np.abs(delta) < 1e-2:
# Changes in x, y, and theta
dtheta = 0
dx = v * np.cos(self.cur_odom_to_base_rot) * dt
dy = v * np.sin(self.cur_odom_to_base_rot) * dt
# New joint values
joint_left_throttle = v * dt / self.CAR_WHEEL_RADIUS
joint_right_throttle = v * dt / self.CAR_WHEEL_RADIUS
joint_left_steer = 0.0
joint_right_steer = 0.0
else:
# Changes in x, y, and theta
tan_delta = np.tan(delta)
dtheta = ((v / self.CAR_LENGTH) * tan_delta) * dt
dx = (self.CAR_LENGTH /
tan_delta) * (np.sin(self.cur_odom_to_base_rot + dtheta) -
np.sin(self.cur_odom_to_base_rot))
dy = (self.CAR_LENGTH / tan_delta) * (
-1 * np.cos(self.cur_odom_to_base_rot + dtheta) +
np.cos(self.cur_odom_to_base_rot))
# New joint values
# Applt kinematic car model to compute wheel deltas
h_val = (self.CAR_LENGTH / tan_delta) - (self.CAR_WIDTH / 2.0)
joint_outer_throttle = (((self.CAR_WIDTH + h_val) /
(0.5 * self.CAR_WIDTH + h_val)) * v * dt /
self.CAR_WHEEL_RADIUS)
joint_inner_throttle = (((h_val) /
(0.5 * self.CAR_WIDTH + h_val)) * v * dt /
self.CAR_WHEEL_RADIUS)
joint_outer_steer = np.arctan2(self.CAR_LENGTH,
self.CAR_WIDTH + h_val)
joint_inner_steer = np.arctan2(self.CAR_LENGTH, h_val)
# Assign joint values according to whether we are turning left or right
if (delta) > 0.0:
joint_left_throttle = joint_inner_throttle
joint_right_throttle = joint_outer_throttle
joint_left_steer = joint_inner_steer
joint_right_steer = joint_outer_steer
else:
joint_left_throttle = joint_outer_throttle
joint_right_throttle = joint_inner_throttle
joint_left_steer = joint_outer_steer - np.pi
joint_right_steer = joint_inner_steer - np.pi
# Apply kinematic model updates and noise to the new pose
new_pose[0] += (dx + np.random.normal(
loc=self.FORWARD_OFFSET, scale=self.FORWARD_FIX_NOISE,
size=1) + np.random.normal(
loc=0.0, scale=np.abs(v) * self.FORWARD_SCALE_NOISE, size=1))
new_pose[1] += (
dy + np.random.normal(
loc=self.SIDE_OFFSET, scale=self.SIDE_FIX_NOISE, size=1) +
np.random.normal(
loc=0.0, scale=np.abs(v) * self.SIDE_SCALE_NOISE, size=1))
new_pose[2] += dtheta + np.random.normal(
loc=self.THETA_OFFSET, scale=self.THETA_FIX_NOISE, size=1)
new_pose[2] = self.clip_angle(new_pose[2])
# Compute the new pose w.r.t the map in meters
new_map_pose = np.zeros(3, dtype=np.float)
new_map_pose[0] = self.cur_map_to_odom_trans[0] + (
new_pose[0] * np.cos(self.cur_map_to_odom_rot) -
new_pose[1] * np.sin(self.cur_map_to_odom_rot))
new_map_pose[1] = self.cur_map_to_odom_trans[1] + (
new_pose[0] * np.sin(self.cur_map_to_odom_rot) +
new_pose[1] * np.cos(self.cur_map_to_odom_rot))
new_map_pose[2] = self.cur_map_to_odom_rot + new_pose[2]
# Get the new pose w.r.t the map in pixels
if self.map_info is not None:
new_map_pose = utils.world_to_map(new_map_pose, self.map_info)
# Update the pose of the car if either bounds checking is not enabled,
# or bounds checking is enabled but the car is in-bounds
new_map_pose_x = int(new_map_pose[0] + 0.5)
new_map_pose_y = int(new_map_pose[1] + 0.5)
if self.permissible_region is None or (
new_map_pose_x >= 0
and new_map_pose_x < self.permissible_region.shape[1]
and new_map_pose_y >= 0
and new_map_pose_y < self.permissible_region.shape[0] and
self.permissible_region[new_map_pose_y, new_map_pose_x] == 1):
# Update pose of base_footprint w.r.t odom
self.cur_odom_to_base_trans[0] = new_pose[0]
self.cur_odom_to_base_trans[1] = new_pose[1]
self.cur_odom_to_base_rot = new_pose[2]
# Update joint values
self.joint_msg.position[0] += joint_left_throttle
self.joint_msg.position[1] += joint_right_throttle
self.joint_msg.position[2] += joint_left_throttle
self.joint_msg.position[3] += joint_right_throttle
self.joint_msg.position[4] = joint_left_steer
self.joint_msg.position[5] = joint_right_steer
# Clip all joint angles
for i in range(len(self.joint_msg.position)):
self.joint_msg.position[i] = self.clip_angle(
self.joint_msg.position[i])
# Publish the tf from odom to base_footprint
self.br.sendTransform(
(self.cur_odom_to_base_trans[0], self.cur_odom_to_base_trans[1],
0.0),
tf.transformations.quaternion_from_euler(
0, 0, self.cur_odom_to_base_rot),
now,
"base_footprint",
"odom",
)
# Publish the joint states
self.joint_msg.header.stamp = now
self.cur_joints_pub.publish(self.joint_msg)
self.last_stamp = now
self.cur_odom_to_base_lock.release()
# Publish current state as a PoseStamped topic
cur_pose = PoseStamped()
cur_pose.header.frame_id = "map"
cur_pose.header.stamp = now
cur_pose.pose.position.x = (self.cur_odom_to_base_trans[0] +
self.cur_map_to_odom_trans[0])
cur_pose.pose.position.y = (self.cur_odom_to_base_trans[1] +
self.cur_map_to_odom_trans[1])
cur_pose.pose.position.z = 0.0
rot = self.cur_odom_to_base_rot + self.cur_map_to_odom_rot
cur_pose.pose.orientation = utils.angle_to_quaternion(rot)
self.state_pub.publish(cur_pose)
def is_pose_reachable(self, pose: Pose) -> bool:
"""Test if a given pose can be reached (lies within the map and is far enough from obstacles)."""
x, y = world_to_map([pose_to_point(pose)], self.gridmap)[0]
return self.gridmap.is_valid_coordinate(x, y) and not self.maze[y][x]
def plan_path(
self,
start: Pose,
goal: Pose,
radius: Optional[float] = None,
simplify: bool = True,
robust: bool = True,
) -> Tuple[Path, float]:
"""Plan a path from start to goal over a given map using A* algorithm.
Parameters
----------
start : Pose
Start position in world coordinates.
goal : Pose
Goal position in world coordinates.
radius : float, optional
The final point in the found path is within the neighborhood of
a given radius from the goal. Default `2 * robot_size`.
simplify : bool, optional
If True (default), simplifies the found path by reducing the amount
of waypoints to the minimal necessary so that the path still doesn't
collide with obstacles.
robust : bool, optional
If True (default), steers A* heuristic to avoid paths that are close to obstacles.
Returns
-------
path : path
The found path.
length : float
Path length (sum of distances between consecutive points).
"""
if radius is None:
radius = ASTAR_TOLERANCE_FACTOR * self.robot_size
radius /= self.gridmap.resolution
# transform start and goal to the useful format
start, goal = pose_to_point(start), pose_to_point(goal)
# transform start and goal from world coordinates to map coordinates
start, goal = world_to_map([start, goal], self.gridmap)
# steer the A* heuristic for robust planning
def steered_heuristic(a: Tuple[int, int], b: Tuple[int, int]) -> float:
return euclidean_distance(
a, b) + OBSTACLE_PENALTY_FACTOR / self.dist_to_obstacles[a[1],
a[0]]
self.astar.heuristic = steered_heuristic if robust else euclidean_distance
# find a path using A* search
path = self.astar.search(self.maze,
start,
goal,
neighborhood="N8",
radius=radius)
# no path found
if len(path) < 2:
return Path(), np.inf
# simplify the found path
if simplify:
path = self._simplify_path(path, self.maze)
# transform the path back to the world coordinates
path = map_to_world(path, self.gridmap)
# create the Path message
path = points_to_path(path)
# compute the path length
length = path_length(path)
return path, length
def __init__(self, T, K, sigma=0.5, _lambda=0.5, roi = None):
if IS_ON_ROBOT:
self.SPEED_TO_ERPM_OFFSET = float(rospy.get_param("/vesc/speed_to_erpm_offset", 0.0))
self.SPEED_TO_ERPM_GAIN = float(rospy.get_param("/vesc/speed_to_erpm_gain", 4614.0))
self.STEERING_TO_SERVO_OFFSET = float(rospy.get_param("/vesc/steering_angle_to_servo_offset", 0.5304))
self.STEERING_TO_SERVO_GAIN = float(rospy.get_param("/vesc/steering_angle_to_servo_gain", -1.2135))
else:
self.SPEED_TO_ERPM_OFFSET = 0.0
self.SPEED_TO_ERPM_GAIN = 4614.0
self.STEERING_TO_SERVO_OFFSET = 0.5304
self.STEERING_TO_SERVO_GAIN = -1.2135
self.CAR_LENGTH = 0.33
self.theta_weight = 0.1
self.bounds_check_weight = 1.0
self.last_pose = None
# MPPI params
self.T = T # Length of rollout horizon
self.K = K # Number of sample rollouts
self.sigma = sigma
self._lambda = _lambda
self.roi = roi
self.was_roi_tape_seen = False
self.goal = None # Lets keep track of the goal pose (world frame) over time
self.lasttime = None
self.last_control = None
# PyTorch / GPU data configuration
# TODO
# you should pre-allocate GPU memory when you can, and re-use it when
# possible for arrays storing your controls or calculated MPPI costs, etc
if CUDA:
self.model = torch.load(MODEL_FILENAME).eval()
else:
self.model = torch.load(MODEL_FILENAME, map_location=lambda storage, loc: storage).eval() # Maps CPU storage and serialized location back to CPU storage
if CUDA:
self.model.cuda() # tell torch to run the network on the GPU
self.dtype = torch.cuda.FloatTensor
else:
self.dtype = torch.FloatTensor
self.Sigma = self.dtype(sigma_data) # Covariance Matrix shape: (CONTROL_SIZE, CONTROL_SIZE)
self.SigmaInv = torch.inverse(self.Sigma)
self.U = self.dtype(CONTROL_SIZE, self.K, self.T).zero_()
self.Epsilon = self.dtype(CONTROL_SIZE, self.K, self.T).zero_()
self.Trajectory_cost = self.dtype(1, self.K).zero_()
self.trajectories = self.dtype(3, self.K, self.T).zero_()
self.noisyU = self.dtype(CONTROL_SIZE, self.K, self.T).zero_()
self.neural_net_input = self.dtype(8).zero_()
self.neural_net_input_torch = self.dtype(self.K, 8).zero_()
self.bounds_check = self.dtype(self.K).zero_()
self.pose_cost = self.dtype(self.K).zero_()
self.omega = self.dtype(1, self.K).zero_()
self.x_tminus1 = self.dtype(self.K, 3).zero_()
self.x_t = self.dtype(self.K, 3).zero_()
self.initial_distance = None
self.intermediate = self.dtype(1,self.K).zero_()
#self.pre_delta_control = self.dtype(CONTROL_SIZE, self.T).zero_()
#self.delta_control = self.dtype(CONTROL_SIZE, self.K, self.T).zero_()
#self.omega_expand = self.dtype(CONTROL_SIZE, self.K, self.T).zero_()
#self.trajectoryMinusMin = self.dtype(1, self.K).zero_()
print("Loading:", MODEL_FILENAME)
print("Model:\n",self.model)
print("Torch Datatype:", self.dtype)
self.U = torch.cuda.FloatTensor(CONTROL_SIZE, self.K, self.T).zero_()
# wtf? #self.Sigma = torch.cuda.FloatTensor(CONTROL_SIZE,CONTROL_SIZE).zero_()
self.Epsilon = torch.cuda.FloatTensor(CONTROL_SIZE, self.K, self.T).zero_()
self.Trajectory_cost = torch.cuda.FloatTensor(self.K, 1).zero_()
# control outputs
self.msgid = 0
# Store last control input
self.last_control = None
# visualization parameters
self.num_viz_paths = 1#40
if self.K < self.num_viz_paths:
self.num_viz_paths = self.K
if IS_ON_ROBOT:
# We will publish control messages and a way to visualize a subset of our
# rollouts, much like the particle filter
self.ctrl_pub = rospy.Publisher(rospy.get_param("~ctrl_topic", "/vesc/high_level/ackermann_cmd_mux/input/nav_0"),
AckermannDriveStamped, queue_size=2)
self.path_pub = rospy.Publisher("/mppi/paths", Path, queue_size = self.num_viz_paths)
self.plan_pub = rospy.Publisher("/mppi/plan_path", Path, queue_size = self.num_viz_paths)
# Use the 'static_map' service (launched by MapServer.launch) to get the map
map_service_name = rospy.get_param("~static_map", "static_map")
print("Getting map from service: ", map_service_name)
rospy.wait_for_service(map_service_name)
map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map # The map, will get passed to init of sensor model
self.map_data = torch.cuda.LongTensor(map_msg.data)
self.map_info = map_msg.info # Save info about map for later use
print("Map Information:\n",self.map_info)
############## FOR FINAL DEMO plan.py
self.currentPlanWaypointIndex = -1
desiredWaypointIndexToExecute = 8
for i in range(desiredWaypointIndexToExecute + 1):
self.advance_to_next_goal()
##################
# Create numpy array representing map for later use
self.map_height = map_msg.info.height
self.map_width = map_msg.info.width
array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width))
self.permissible_region = np.zeros_like(array_255, dtype=bool)
self.permissible_region[array_255==0] = 1 # Numpy array of dimension (map_msg.info.height, map_msg.info.width),
# With values 0: not permissible, 1: permissible
#self.permissible_region = np.negative(self.permissible_region) # 0 is permissible, 1 is not
planx = (2600,1880,1435,1250,540)
plany = (660,440,545,460,835)
i = 0
particalx = 1000
particaly = 1000
if particalx == planx[i] and particaly == plany[i]:
i += 1
current_waypoint = planx[i],plany[i]
grid_poses = planx[i],plany[i], 0
print grid_poses
Utils.world_to_map(grid_poses, self.map_info)
print current_waypoint
particalx = 2600
particaly = 660
if particalx == planx[i] and particaly == plany[i]:
i += 1
current_waypoint = planx[i],plany[i]
print current_waypoint
pause()
# csvfile = open(CSV_FILE, 'r')
# # reader = csv.reader(csvfile, delimiter=',')
# # len_csv = sum(1 for row in reader)
# # badpoints = np.zeros((len_csv - 1, 2), dtype=np.int32)
# firstLine = True
# # index = 0
# reader = csv.reader(csvfile, delimiter=',')
# badpoints = None
# for row in reader:
# if firstLine:
# firstLine = False
# continue
# if badpoints is None:
# print('C')
# badpoints = np.array([[int(row[0]), self.map_height - int(row[1])]], dtype=np.int32)
# else:
# badpoints = np.append(badpoints, np.array([[int(row[0]), self.map_height - int(row[1])]], dtype=np.int32), axis=0)
#
badpoints = None
with open(CSV_FILE, 'r') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
firstLine = True
for row in reader:
if firstLine:
firstLine = False
continue
if badpoints is None:
print('C')
badpoints = np.array([[int(row[0]), self.map_height - int(row[1])]], dtype=np.int32)
else:
badpoints = np.append(badpoints, np.array([[int(row[0]), int(row[1])]], dtype=np.int32), axis=0)
print('Badpoints: ', badpoints)
redBufferSpace = 10
for i in range(0, redBufferSpace+1):
plus0_indices = np.minimum(badpoints[:, 0] + i, self.permissible_region.shape[0] - 1)
minus0_indices = np.maximum(badpoints[:, 0] - i, 0)
for j in range(0, redBufferSpace+1):
minus1_indices = np.maximum(badpoints[:,1]+j, 0)
plus1_indices = np.minimum(badpoints[:,1] - j, self.permissible_region.shape[1] -1)
self.permissible_region[minus0_indices, plus1_indices] = 1
self.permissible_region[plus0_indices, minus1_indices] = 1
self.permissible_region[plus0_indices, plus1_indices] = 1
self.permissible_region[minus0_indices, minus1_indices] = 1
print('Sum in permissible region before: ', np.sum(self.permissible_region))
indices = np.argwhere(array_255 == 100)
bufferSize = 16 # Tune this
for i in range(0, bufferSize + 1): # Create buffer between car and walls
print('i: ', i)
plus0_indices = np.minimum(indices[:, 0] + i, self.permissible_region.shape[0] - 1)
minus0_indices = np.maximum(indices[:, 0] - i, 0)
for j in range(0, bufferSize+1):
minus1_indices = np.maximum(indices[:,1]-j, 0)
plus1_indices = np.minimum(indices[:,1] - j, self.permissible_region.shape[1] -1)
self.permissible_region[minus0_indices, plus1_indices] = 1
self.permissible_region[plus0_indices, minus1_indices] = 1
self.permissible_region[plus0_indices, plus1_indices] = 1
self.permissible_region[minus0_indices, minus1_indices] = 1
print('Sum in permissible region after: ', np.sum(self.permissible_region))
self.permissible_region_torch = torch.from_numpy(
self.permissible_region.astype(float)
).type(self.dtype)
print("Making callbacks")
self.goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped, self.clicked_goal_cb, queue_size=1)
self.pose_sub = rospy.Subscriber("/pf/ta/viz/inferred_pose",
PoseStamped, self.mppi_cb, queue_size=1)
'''start_pose = np.array([[1490, 570, 1.85]])
waypoints = np.array([[600, 835, 4.0 ],
[600, 700, 5.0 ],
[2500, 640, 6.0]])'''
start_pose1 = np.array([32.7, 11.3, 4])
waypoints1 = np.array([30.52, 11.05, 2])
plan = []
map_img, map_info = utils.get_map(MAP_TOPIC)
'''Below 8 lines were used to find the intermediate waypoints to create an optimal path'''
map_to_world(start_pose, map_info)
map_to_world(waypoints, map_info)
start_pose1 = utils.world_to_map(start_pose1, map_info)
waypoints1 = utils.world_to_map(waypoints1, map_info)
print('start_pose1:')
print(start_pose1)
print('waypoints1:')
print(waypoints1)
pp = PathPlanner(plan, waypoints)
# offline plan location
offline_path = '/home/car-user/racecar_ws/src/final/offline_paths/final_refined.npy'
# check if offline plan already exists. if yes, do not continue
if os.path.isfile(offline_path):
