def createPointMarker2(self, points, marker_id, rgba = None, pose=[0,0,0,0,0,0,1], frame_id = '/world'):
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.scale.x = 0.01
marker.scale.y = 0.01
marker.scale.z = 0.01
marker.id = marker_id
n = len(points)
sub = 1
if rgba is not None:
marker.color.r, marker.color.g, marker.color.b, marker.color.a = tuple(rgba)
for i in xrange(0,n,sub):
p = Point()
p.x = points[i][0]
p.y = points[i][1]
p.z = points[i][2]
marker.points.append(p)
if rgba is None:
for i in xrange(0,n,sub):
p = ColorRGBA()
p.r = points[i][3]
p.g = points[i][4]
p.b = points[i][5]
p.a = points[i][6]
marker.colors.append(p)
# marker.pose = poselist2pose(pose)
return marker
def recur(node):
if self.enable_vis:
TEMP = Point()
TEMP.x = node.value[0]
TEMP.y = node.value[1]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.trajectory.points.append([node.value[0], node.value[1]])
parent = node.parent
if parent is not None:
recur(parent)
def marker_stuff(self, x_obs, y_obs):
marker = Marker()
marker.header.frame_id = "map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.5
marker.scale.y = 0.5
for i in range(len(x_obs)):
TEMP = Point()
TEMP.x = x_obs[i]
TEMP.y = y_obs[i]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.vis_pub.publish(marker)
def lidar_cb(self, msg):
rospy.loginfo("starting lidar cb")
if self.road_map is None:
return
rospy.loginfo("got map")
ranges = np.array(msg.ranges)
num_ranges = len(ranges)
angles = (np.ones(num_ranges) * msg.angle_min) + (
np.arange(num_ranges) * msg.angle_increment)
# Discard extraneous data (< range_min or > range_max)
angles = angles[ranges > msg.range_min]
ranges = ranges[ranges > msg.range_min]
angles = angles[ranges < msg.range_max]
ranges = ranges[ranges < msg.range_max]
# Remove data points not in the viable range in front of the car, ie theta E (-viable_angle_range, viable_angle_range)
ranges = ranges[angles > -self.viable_angle_range]
angles = angles[angles > -self.viable_angle_range]
ranges = ranges[angles < self.viable_angle_range]
angles = angles[angles < self.viable_angle_range]
# Remove data points outside self.in_range
angles = angles[ranges < self.in_range]
ranges = ranges[ranges < self.in_range]
# Convert to cartesian, LIDAR-frame positions
x = ranges * np.cos(angles)
y = ranges * np.sin(angles)
new_num_ranges = len(x)
arr = np.vstack((x, y))
# Find transform from LIDAR frame to map frame
try:
(trans,
rot) = self.listener.lookupTransform(msg.header.frame_id,
self.road_map.frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rate.sleep()
return
# Convert translation + quaternion to homogeneous transformation matrix
T_lidar_map = transformations.quaternion_matrix(rot)
T_lidar_map = T_lidar_map.T
TEMP = transformations.quaternion_matrix(rot)
TEMP = np.linalg.inv(TEMP[:3, :3])
T_lidar_map[:3, 3] = np.matmul(TEMP, -np.array(trans).T)
#T_lidar_map = T_lidar_map.T
# Convert LIDAR points from LIDAR frame to map frame
arr_LIDAR = np.vstack(
(x, y, np.zeros(new_num_ranges), np.ones(new_num_ranges)))
arr_MAP = np.matmul(T_lidar_map, arr_LIDAR)
print(T_lidar_map)
arr_MAP = arr_MAP[:2]
x_map = arr_MAP[0]
y_map = arr_MAP[1]
# Find which points are on the roads
on_road = self.road_map.check_free(arr_MAP)
x_obs = x_map[on_road]
y_obs = y_map[on_road]
"""
# Find occupancy grid matrix with detected obstacles filled in
new_occ_matrix = self.road_map.add_obstructions(np.vstack((x_obs, y_obs)))
# Creates and publishes OccupancyGrid message with obstacles to /obstacles
obs_msg = OccupancyGrid()
# now = rospy.get_time()
# obs_msg.header.stamp = now
obs_msg.header.frame_id = self.road_map.frame
obs_msg.info = self.road_map.msg.info
# obs_msg.info.map_load_time = rospy.Time(0) # Probably unnecessary?
obs_msg.data = np.ravel(new_occ_matrix)
rospy.loginfo("publishing")
self.obs_pub.publish(obs_msg)
print("test")
"""
marker = Marker()
marker.header.frame_id = "map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.5
marker.scale.y = 0.5
for i in range(len(x_obs)):
TEMP = Point()
TEMP.x = x_obs[i]
TEMP.y = y_obs[i]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.vis_pub.publish(marker)
def plan_path(self, start_point, end_point, map_obj):
"""
RRT*. Tries to find a collision free path from start to goal.
"""
# STUFF FOR TESTING
self.trajectory.clear()
if self.enable_vis:
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
self.vis_pub.publish(marker)
exploration_bias = 1.0 - self.goal_bias
final_node = None
num_existing_path_points_added = 0
self.rrt_star = RRTStar(Node(start_point))
self.max_iterations = self.rrt_star.max_size
while self.rrt_star.size <= self.max_iterations:
p = np.random.uniform()
if p < exploration_bias:
x_rand = self.map.sample_free_space()
else:
if final_node is None:
x_rand = end_point
else:
x_rand = self.branched_from_existing_path(
final_node,
depth_underestimate=num_existing_path_points_added)
num_existing_path_points_added += 1
x_nearest = self.rrt_star.nearest(
x_rand) # Find the nearest node to x_rand
path = self.map.generate_line_path(x_nearest.value,
x_rand,
eta=self.eta)
if path is not None: # no obstacles between x_nearest and x_rand
x_new = path[-1]
X_nearby_connectable = self.find_nearby_connectable(
x_nearest, x_new)
cost_min, node_min = self.find_best_parent(
X_nearby_connectable, x_new)
X_nearby_connectable.remove(
node_min
) # Remove x_new's parent node from the list of nearby nodes so it is not considered for rewiring
# Create the new node at x_new!
node_new = self.rrt_star.add_config(node_min, x_new)
if self.enable_vis:
# FOR TESTING ONLY #
# Code to publish marker for new node
###########################################################################################
TEMP = Point()
TEMP.x = x_new[0]
TEMP.y = x_new[1]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.vis_pub.publish(marker)
###########################################################################################
self.rewire(cost_min, node_new, X_nearby_connectable)
if np.allclose(node_new.value, end_point, .05, 0) and (
final_node is
None): #np.array_equal(node_new.value, end_point):
final_node = node_new
# reduce exploration bias so that we reinforce the existing path
exploration_bias = .5
if VERBOSE:
print("Path found!!!!")
print(final_node.cost)
if rospy.get_time() - self.start_time > self.time_thresh:
if VERBOSE:
print(self.rrt_star.size)
break
if final_node is not None:
if self.enable_vis:
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.points = []
marker.colors = []
def recur(node):
if self.enable_vis:
TEMP = Point()
TEMP.x = node.value[0]
TEMP.y = node.value[1]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.trajectory.points.append([node.value[0], node.value[1]])
parent = node.parent
if parent is not None:
recur(parent)
recur(final_node)
self.trajectory.points.reverse()
if self.enable_vis:
self.vis_pub.publish(marker)
if VERBOSE:
print(final_node.depth)
else:
# TODO:give best guess- find closest node to goal
if VERBOSE:
print("No path found! Please try again.")
