def test_roundtrip_numpy(self):
points_arr = self.makeArray(100)
cloud_msg = ros_numpy.msgify(numpy_msg(PointCloud2), points_arr)
new_points_arr = ros_numpy.numpify(cloud_msg)
np.testing.assert_equal(points_arr, new_points_arr)
def map_timer_callback(self, event):
if self.debug_merged_map:
map_msg = ros_numpy.msgify(ros_numpy.numpy_msg(OccupancyGrid), self.map.grid, info=self.map.info)
map_msg.header = Header()
map_msg.header.stamp = rospy.Time.now()
map_msg.header.frame_id = self.map.frame
self.pub_merged_map.publish(map_msg)
def process_depth_callback(data):
try:
image = bridge.imgmsg_to_cv2(data, "passthrough")
except CvBridgeError as e:
print(e)
depth = np.asarray(image)
good_indices = np.where(depth > 1)
row_coords = good_indices[0]
column_coords = good_indices[1]
random_mask = np.random.choice([False, True],
len(row_coords),
p=[0.90, 0.10])
row_coords = row_coords[random_mask]
column_coords = column_coords[random_mask]
masked_depth = depth[row_coords, column_coords]
xyz_array = np.zeros((len(row_coords), 3))
xyz_array[:, 0] = (column_coords - camera.cx()) / (camera.fx() * 1000.0)
xyz_array[:, 1] = (row_coords - camera.cy()) / (camera.fy() * 1000.0)
xyz_array[:, 0] = np.multiply(xyz_array[:, 0], masked_depth)
xyz_array[:, 1] = np.multiply(xyz_array[:, 1], masked_depth)
xyz_array[:, 2] = masked_depth / 1000.0
array_for_conversion = np.zeros(
(len(row_coords), ),
dtype=[('x', np.float32), ('y', np.float32), ('z', np.float32),
('r', np.uint8), ('g', np.uint8), ('b', np.uint8)])
array_for_conversion['x'] = xyz_array[:, 0]
array_for_conversion['y'] = xyz_array[:, 1]
array_for_conversion['z'] = xyz_array[:, 2]
#print(xyz_array)
#print(xyz_array.shape)
#print(xyz_array.dtype)
xyz_array = np.transpose(xyz_array)
cloud_msg = ros_numpy.msgify(numpy_msg(PointCloud2), array_for_conversion)
print(data.header)
cloud_msg.header = data.header
pc_pub.publish(cloud_msg)
def _make_local_map(self, scan):
#target frame is map_frame, source frame is laser_link
trans = self.tf_buffer.lookup_transform(
target_frame=self.map_frame,
source_frame=scan.header.frame_id,
time=scan.header.stamp,
timeout=rospy.Duration(5)
)
pos = trans.transform.translation
orient = trans.transform.rotation
# transform from base to map
transform = np.dot(
transformations.translation_matrix([pos.x, pos.y, pos.z]),
transformations.quaternion_matrix([orient.x, orient.y, orient.z, orient.w])
)
self.map_info.origin.position.x = pos.x - self.map_info.resolution*self.width/2.0
self.map_info.origin.position.y = pos.y - self.map_info.resolution*self.height/2.0
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mask = (scan.ranges < scan.range_max) & (scan.ranges > scan.range_min)
angles = np.arange(scan.angle_min,scan.angle_max,scan.angle_increment)
x = scan.ranges[mask]*np.cos(angles[mask])
y = scan.ranges[mask]*np.sin(angles[mask])
# set the last component to zero to ignore translation
ii, jj = mcl_local_map.index_at(
np.vstack((x,y,np.zeros(np.sum(mask)),np.ones(np.sum(mask)))).T,
world_to_map=transform
)
ok = (
(jj >= 0) & (jj < self.width) &
(ii >= 0) & (ii < self.height)
)
ii = ii[ok]
jj = jj[ok]
mcl_local_map.grid[ii,jj] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid), mcl_local_map.grid, info=self.map_info)
map_msg.header = scan.header
map_msg.header.frame_id = 'map'
return map_msg
def _make_local_map(self, point):
#target frame is map_frame, source frame is laser_link
self.map_info.origin.position = point.point
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mcl_local_map.grid[:,:] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid), mcl_local_map.grid, info=self.map_info)
map_msg.header = point.header
map_msg.header.frame_id = 'map'
return map_msg
def _make_local_map(self, point):
#target frame is map_frame, source frame is laser_link
self.map_info.origin.position = point.point
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mcl_local_map.grid[:, :] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid),
mcl_local_map.grid,
info=self.map_info)
map_msg.header = point.header
map_msg.header.frame_id = 'map'
return map_msg
def _make_local_map(self, scan):
#target frame is map_frame, source frame is laser_link
trans = self.tf_buffer.lookup_transform(
target_frame=self.map_frame,
source_frame=scan.header.frame_id,
time=scan.header.stamp,
timeout=rospy.Duration(5))
pos = trans.transform.translation
orient = trans.transform.rotation
# transform from base to map
transform = np.dot(
transformations.translation_matrix([pos.x, pos.y, pos.z]),
transformations.quaternion_matrix(
[orient.x, orient.y, orient.z, orient.w]))
self.map_info.origin.position.x = pos.x - self.map_info.resolution * self.width / 2.0
self.map_info.origin.position.y = pos.y - self.map_info.resolution * self.height / 2.0
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mask = (scan.ranges < scan.range_max) & (scan.ranges > scan.range_min)
angles = np.arange(scan.angle_min, scan.angle_max,
scan.angle_increment)
x = scan.ranges[mask] * np.cos(angles[mask])
y = scan.ranges[mask] * np.sin(angles[mask])
# set the last component to zero to ignore translation
ii, jj = mcl_local_map.index_at(np.vstack(
(x, y, np.zeros(np.sum(mask)), np.ones(np.sum(mask)))).T,
world_to_map=transform)
ok = ((jj >= 0) & (jj < self.width) & (ii >= 0) & (ii < self.height))
ii = ii[ok]
jj = jj[ok]
mcl_local_map.grid[ii, jj] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid),
mcl_local_map.grid,
info=self.map_info)
map_msg.header = scan.header
map_msg.header.frame_id = 'map'
return map_msg
def sub_map(self, map):
"""
We need the global map in order to set up the coordinate frame
correctly for the local map
"""
rospy.loginfo("received map")
self.mcl_map = mcl.Map(map)
self.map_frame = map.header.frame_id
self.map_info_glob = map.info
self.width = 200
self.height = 200
ros_map = numpy_msg(OccupancyGrid)()
ros_map.header.frame_id = 'map'
ros_map.info = deepcopy(map.info)
ros_map.info.width = self.width
ros_map.info.height = self.height
ros_map.info.origin.position.x = -1*ros_map.info.resolution*self.height/2.0
ros_map.info.origin.position.y = -1*ros_map.info.resolution*self.width/2.0
self.map_info = ros_map.info
ros_map.data = np.ones(self.width*self.height, dtype=np.int8) * -1
self.ros_map = ros_map
def sub_map(self, map):
"""
We need the global map in order to set up the coordinate frame
correctly for the local map
"""
rospy.loginfo("received map")
self.mcl_map = mcl.Map(map)
self.map_frame = map.header.frame_id
self.map_info_glob = map.info
self.width = 200
self.height = 200
ros_map = numpy_msg(OccupancyGrid)()
ros_map.header.frame_id = 'map'
ros_map.info = deepcopy(map.info)
ros_map.info.width = self.width
ros_map.info.height = self.height
ros_map.info.origin.position.x = -1 * ros_map.info.resolution * self.height / 2.0
ros_map.info.origin.position.y = -1 * ros_map.info.resolution * self.width / 2.0
self.map_info = ros_map.info
ros_map.data = np.ones(self.width * self.height, dtype=np.int8) * -1
self.ros_map = ros_map
def __init__(self, ros_map=None, map_info=None, frame=None, grid=None):
# for backwards compatibility, accept a OccupancyGrid
if ros_map is not None:
assert isinstance(
ros_map, (OccupancyGrid, ros_numpy.numpy_msg(OccupancyGrid)))
if map_info is not None or frame is not None or grid is not None:
raise ValueError(
'If ros_map is specified, it must be the only argument')
map_info = ros_map.info
frame = ros_map.header.frame_id
grid = ros_numpy.numpify(ros_map)
else:
assert map_info is not None
assert frame is not None
assert grid is not None
self.info = map_info
self.frame = frame
self.grid = grid
self.resolution = map_info.resolution
self.pos = map_info.origin.position
self.orient = map_info.origin.orientation
# transform from index to world
self.transform = np.dot(
transformations.translation_matrix(
[self.pos.x, self.pos.y, self.pos.z]),
transformations.quaternion_matrix([
self.orient.x, self.orient.y, self.orient.z, self.orient.w
])).dot(transformations.scale_matrix(map_info.resolution))
idxs = [0, 1, 3]
self.inv_transform = np.linalg.inv(self.transform)[idxs, :]
self.transform = self.transform[:, idxs]
def __init__(self, ros_map=None, map_info=None, frame=None, grid=None):
# for backwards compatibility, accept a OccupancyGrid
if ros_map is not None:
assert isinstance(ros_map, (OccupancyGrid, ros_numpy.numpy_msg(OccupancyGrid)))
if map_info is not None or frame is not None or grid is not None:
raise ValueError('If ros_map is specified, it must be the only argument')
map_info = ros_map.info
frame = ros_map.header.frame_id
grid = ros_numpy.numpify(ros_map)
else:
assert map_info is not None
assert frame is not None
assert grid is not None
self.info = map_info
self.frame = frame
self.grid = grid
self.resolution = map_info.resolution
self.pos = map_info.origin.position
self.orient = map_info.origin.orientation
# transform from index to world
self.transform = np.dot(
transformations.translation_matrix([self.pos.x, self.pos.y, self.pos.z]),
transformations.quaternion_matrix([self.orient.x, self.orient.y, self.orient.z, self.orient.w])
).dot(
transformations.scale_matrix(map_info.resolution)
)
idxs = [0, 1, 3]
self.inv_transform = np.linalg.inv(self.transform)[idxs,:]
self.transform = self.transform[:,idxs]
class LocalMapper(Node):
map_pub = Publisher('~local_map', numpy_msg(OccupancyGrid), queue_size=1)
def __init__(self):
rospy.loginfo("created node")
self.mcl_map = None
self.mcl_local_map = None
self.map_frame = None
self.map_msg = None
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
super(LocalMapper, self).__init__()
@Subscriber('~global_map', numpy_msg(OccupancyGrid), queue_size=1)
def sub_map(self, map):
"""
We need the global map in order to set up the coordinate frame
correctly for the local map
"""
rospy.loginfo("received map")
self.mcl_map = mcl.Map(map)
self.map_frame = map.header.frame_id
self.map_info_glob = map.info
self.width = 200
self.height = 200
ros_map = numpy_msg(OccupancyGrid)()
ros_map.header.frame_id = 'map'
ros_map.info = deepcopy(map.info)
ros_map.info.width = self.width
ros_map.info.height = self.height
ros_map.info.origin.position.x = -1 * ros_map.info.resolution * self.height / 2.0
ros_map.info.origin.position.y = -1 * ros_map.info.resolution * self.width / 2.0
self.map_info = ros_map.info
ros_map.data = np.ones(self.width * self.height, dtype=np.int8) * -1
self.ros_map = ros_map
def _make_local_map(self, scan):
#target frame is map_frame, source frame is laser_link
trans = self.tf_buffer.lookup_transform(
target_frame=self.map_frame,
source_frame=scan.header.frame_id,
time=scan.header.stamp,
timeout=rospy.Duration(5))
pos = trans.transform.translation
orient = trans.transform.rotation
# transform from base to map
transform = np.dot(
transformations.translation_matrix([pos.x, pos.y, pos.z]),
transformations.quaternion_matrix(
[orient.x, orient.y, orient.z, orient.w]))
self.map_info.origin.position.x = pos.x - self.map_info.resolution * self.width / 2.0
self.map_info.origin.position.y = pos.y - self.map_info.resolution * self.height / 2.0
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mask = (scan.ranges < scan.range_max) & (scan.ranges > scan.range_min)
angles = np.arange(scan.angle_min, scan.angle_max,
scan.angle_increment)
x = scan.ranges[mask] * np.cos(angles[mask])
y = scan.ranges[mask] * np.sin(angles[mask])
# set the last component to zero to ignore translation
ii, jj = mcl_local_map.index_at(np.vstack(
(x, y, np.zeros(np.sum(mask)), np.ones(np.sum(mask)))).T,
world_to_map=transform)
ok = ((jj >= 0) & (jj < self.width) & (ii >= 0) & (ii < self.height))
ii = ii[ok]
jj = jj[ok]
mcl_local_map.grid[ii, jj] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid),
mcl_local_map.grid,
info=self.map_info)
map_msg.header = scan.header
map_msg.header.frame_id = 'map'
return map_msg
@Subscriber('~scan', numpy_msg(LaserScan), queue_size=1)
def sub_scan(self, scan):
if self.mcl_map is None:
return
delay = (scan.header.stamp - rospy.Time.now()).to_sec()
rospy.loginfo('scan stamp, now = {}'.format(delay))
if delay < -.2:
return
pr = cProfile.Profile()
pr.enable()
self.map_msg = self._make_local_map(scan)
rospy.loginfo("updated map")
self.map_pub.publish(self.map_msg)
rospy.loginfo('published map')
pr.disable()
pr.dump_stats(
os.path.join(rospkg.RosPack().get_path('lab6'), 'grid_stats.txt'))
rospy.loginfo("wrote stats")
class FakeCone(Node):
map_pub = Publisher('~local_map', numpy_msg(OccupancyGrid), queue_size=1)
def __init__(self):
rospy.loginfo("created node")
self.mcl_map = None
self.mcl_local_map = None
self.map_frame = None
self.map_msg = None
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
super(FakeCone, self).__init__()
@Subscriber('~global_map', numpy_msg(OccupancyGrid), queue_size=1)
def sub_map(self, map):
"""
We need the global map in order to set up the coordinate frame
correctly for the local map
"""
rospy.loginfo("received map")
self.mcl_map = mcl.Map(map)
self.map_frame = map.header.frame_id
self.map_info_glob = map.info
self.width = 10
self.height = 10
ros_map = numpy_msg(OccupancyGrid)()
ros_map.header.frame_id = 'map'
ros_map.info = deepcopy(map.info)
ros_map.info.width = self.width
ros_map.info.height = self.height
ros_map.info.origin.position.x = -1 * ros_map.info.resolution * self.height / 2.0
ros_map.info.origin.position.y = -1 * ros_map.info.resolution * self.width / 2.0
self.map_info = ros_map.info
ros_map.data = np.ones(self.width * self.height, dtype=np.int8) * -1
self.ros_map = ros_map
def _make_local_map(self, point):
#target frame is map_frame, source frame is laser_link
self.map_info.origin.position = point.point
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mcl_local_map.grid[:, :] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid),
mcl_local_map.grid,
info=self.map_info)
map_msg.header = point.header
map_msg.header.frame_id = 'map'
return map_msg
@Subscriber('/clicked_point', PointStamped, queue_size=1)
def sub_scan(self, point):
if self.mcl_map is None:
return
pr = cProfile.Profile()
pr.enable()
self.map_msg = self._make_local_map(point)
rospy.loginfo("updated map")
self.map_pub.publish(self.map_msg)
rospy.loginfo('published map')
pr.disable()
pr.dump_stats(
os.path.join(rospkg.RosPack().get_path('lab6'), 'grid_stats.txt'))
rospy.loginfo("wrote stats")
class PlanNode(Node):
max_diff = Param(int, default=20)
target_pose_pub = Publisher('~target_pose', PoseStamped, queue_size=1)
TARGET_DIST = 2
TURN_RADIUS = 1
UPDATE_HZ = 4
CLOSE_THRESH = 5
TARGET_OFFSET = 0 # the location on the car that we try to match with reality
def __init__(self):
rospy.loginfo("created node")
self.global_mcl_map = None
self.map_frame = None
self.scan_target_pose = None
self.map_target_pose = None
self.marker_target_pose = None
self.path = None
self.path_index = None
self.pub_path_point = True
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
super(PlanNode, self).__init__()
@Subscriber('~map', numpy_msg(OccupancyGrid), queue_size=1)
def sub_map(self, map):
self.map_frame = map.header.frame_id
rospy.loginfo("received map")
@Subscriber('~path', Path, queue_size=1)
def sub_path(self, path):
self.path = path.poses
self.path_index = 0
self.pub_path_point = True
rospy.loginfo([(i.pose.position.x,i.pose.position.y) for i in path.poses])
rospy.loginfo("received path")
@Subscriber('~scan', numpy_msg(LaserScan), queue_size=1)
def sub_scan(self, scan):
if self.global_mcl_map is None or self.map_frame is None:
return
delay = (scan.header.stamp-rospy.Time.now()).to_sec()
#rospy.loginfo('scan stamp, now = {}'.format(delay))
if delay < -.2:
return
far = scan.ranges>10
buffer_size = 10
far_buffered = np.convolve(far, np.ones(buffer_size).astype(bool))[0:len(far)]
changes = np.where(far_buffered[:-1] != far_buffered[1:])[0]
if len(changes) == 0:
return
group_sizes = np.diff(changes)[::2]
max_group = np.argmax(group_sizes)
target_index = (changes[2*max_group]+changes[2*max_group+1]-buffer_size)/2
rel_angle = scan.angle_min + target_index*scan.angle_increment
trans = self.tf_buffer.lookup_transform(
target_frame=self.map_frame,
source_frame=scan.header.frame_id,
time=scan.header.stamp,
timeout=rospy.Duration(1)
)
pos = trans.transform.translation
orient = trans.transform.rotation
# transform from scan to map
transform = numpify(trans.transform)
target_vec = self.TARGET_DIST * np.array([
np.cos(rel_angle),
np.sin(rel_angle),
0,
1
]).dot(transform.T)
target_angle = model.pose_from_ros(trans).theta + rel_angle
scan_target_pose = PoseStamped()
scan_target_pose.header = scan.header
scan_target_pose.header.frame_id = self.map_frame
scan_target_pose.pose = Pose(
Point(*target_vec[0:3]),
Quaternion(0,0,np.sin(.5*target_angle),np.cos(.5*target_angle))
)
self.scan_target_pose = scan_target_pose
rospy.loginfo("updated target pose")
@Subscriber('~marker_poses', PointStamped, queue_size=1)
def sub_marker(self, marker):
marker_loc = numpify(marker.point, hom=True)
if np.isnan(marker_loc).any():
self.marker_target_pose = None
return
trans = self.tf_buffer.lookup_transform(
target_frame="base_link",
source_frame=self.map_frame,
time=marker.header.stamp,
timeout=rospy.Duration(1)
)
transform = numpify(trans.transform)
marker_loc = marker_loc.dot(transform.T)
marker_target_pose = PoseStamped()
marker_target_pose.header = marker.header
marker_target_pose.header.frame_id = 'map'
marker_target_pose.pose = Pose(
msgify(Point, marker_loc),
Quaternion(0,0,0,1)
)
self.marker_target_pose = marker_target_pose
@Timer(rospy.Duration(.25))
def timer_callback(self, timer):
rospy.loginfo("Top Planner pub loop")
try:
trans = self.tf_buffer.lookup_transform(
target_frame=self.map_frame,
source_frame="base_link",
time=timer.current_real,
timeout=rospy.Duration(1)
)
except Exception:
rospy.loginfo("Top Planner pub loop: tf from base link to map not found")
return
at = model.pose_from_ros(trans)
if np.isnan(at.x) or np.isnan(at.y) or np.isnan(at.theta):
return
if self.path is None:
return
while self.path_index < len(self.path):
map_target_pose = self.path[self.path_index]
t = model.pose_from_ros(map_target_pose)
## If already achieved a point or if a point is not ahead and within turning radius, look at future points in the path
if self.is_achieved(at, t) or not self.is_feasible(at, t):
rospy.loginfo("passed index {}".format(self.path_index))
self.path_index += 1
self.pub_path_point = True
else:
break
if self.marker_target_pose is not None and (self.is_achieved(at,
model.pose(
self.marker_target_pose.pose.position.x,
self.marker_target_pose.pose.position.y,
at.theta
)) or not self.is_feasible(at,
model.pose(
self.marker_target_pose.pose.position.x,
self.marker_target_pose.pose.position.y,
at.theta
))):
self.marker_target_pose = None
rospy.loginfo("path index {} publish? {}".format(self.path_index, self.pub_path_point))
if self.marker_target_pose is not None:
self.target_pose_pub.publish(self.marker_target_pose)
self.pub_path_point = True
elif self.path is not None and self.path_index < len(self.path):
if self.pub_path_point:
rospy.loginfo("publishing path target pose")
self.target_pose_pub.publish(self.path[self.path_index])
self.pub_path_point = False
elif self.scan_target_pose is not None:
self.target_pose_pub.publish(self.scan_target_pose)
def is_feasible(self, rx, ry, rt, tx, ty, tt):
dist = np.hypot(rx-tx, ry-ty)
angle = (np.arctan2(ty-ry, tx-rx)-rt)%(2*np.pi)
def is_feasible(self, r, t):
dist = np.hypot(r.x-t.x, r.y-t.y)
angle = (np.arctan2(t.y-r.y, t.x-r.x)-r.theta)%(2*np.pi)
if angle > np.pi:
angle -= 2*np.pi
## TODO: better feasibility metric?
if angle > np.pi/2 or angle < -np.pi/2:
return False
return dist/2/np.cos(np.pi/2-np.abs(angle)) > self.TURN_RADIUS
def is_achieved(self, r, t):
nr = np.array([r.x,r.y]) + self.TARGET_OFFSET*np.array([np.cos(r.theta),np.sin(r.theta)])
nt = np.array([t.x,t.y]) + self.TARGET_OFFSET*np.array([np.cos(t.theta),np.sin(t.theta)])
dist = np.hypot(nr[0]-nt[0], nr[1]-nt[1])
rospy.loginfo("rxy {} {} txy {} {} dist {}".format(nr[0], nr[1], nt[0], nt[1], dist))
return dist < self.CLOSE_THRESH
class RRTNode(Node):
pub_vis_tree = Publisher('/vis/tree', MarkerArray, queue_size=1)
pub_vis_tree_wip = Publisher('/vis/tree_wip', MarkerArray, queue_size=1)
pub_vis_sample = Publisher('/vis/new_sample', Marker, queue_size=1)
# WARNING. DEBUG ONLY! VERY CPU INTENSIVE!
pub_merged_map = Publisher('/vis/merged_map', ros_numpy.numpy_msg(OccupancyGrid), queue_size=1)
pub_path = Publisher('~path', Path, queue_size=1)
base_frame = Param(str, default='base_link')
replan = Param(str, default='always')
allow_reverse = Param(bool, default=False)
reroute_threshold = Param(float, default=0.2)
debug_merged_map = Param(bool, default=False)
use_ackermann_rrt = Param(bool, default=False)
use_ackermann_rrt2 = Param(bool, default=False)
do_profile = Param(bool, default=False)
def __init__(self):
self.map = None
super(RRTNode, self).__init__()
assert self.replan in ('on_goal_change', 'always')
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.goal = None
self.goal_changed = False
if self.use_ackermann_rrt2:
rospy.loginfo("Using AckermannRRT2")
elif self.use_ackermann_rrt:
rospy.loginfo("Using AckermannRRT")
else:
rospy.loginfo("Using OmniRRT")
self.rrt = None
def start_over(self, pose):
hooks = rrtutils.RvizHooks(
pub_sample=self.pub_vis_sample,
pub_tree_wip=self.pub_vis_tree_wip,
frame=self.map.frame
)
if self.use_ackermann_rrt2:
self.rrt = AckermannRRT2(self.map, pose, self.goal, allow_reverse=self.allow_reverse, hooks=hooks)
elif self.use_ackermann_rrt:
self.rrt = AckermannRRT(self.map, pose, self.goal, allow_reverse=self.allow_reverse, hooks=hooks)
else:
self.rrt = OmniRRT(self.map, pose, self.goal, hooks=hooks)
@Subscriber('~goal', PoseStamped)
def sub_pose(self, pose):
if self.use_ackermann_rrt2:
goal = model.pose_from_ros(pose)
else:
goal = ros_numpy.numpify(pose.pose.position)[:2]
if self.goal is None:
self.goal = goal
else:
# update in place, so the rrt gets the new value automatically
self.goal[...] = goal
self.goal_changed = True
rospy.loginfo("Updated goal to {}".format(self.goal))
@Subscriber('~map', rospy.numpy_msg.numpy_msg(OccupancyGrid))
def sub_map(self, map):
self.map = mcl.HybridMap(map)
@Subscriber('~local_map', rospy.numpy_msg.numpy_msg(OccupancyGrid))
def sub_localmap(self, map):
if self.map is not None:
self.map.merge_transient(mcl.Map(map))
# For debugging, we should publish our merged map every once in a while
@Timer(rospy.Duration(2.0))
def map_timer_callback(self, event):
if self.debug_merged_map:
map_msg = ros_numpy.msgify(ros_numpy.numpy_msg(OccupancyGrid), self.map.grid, info=self.map.info)
map_msg.header = Header()
map_msg.header.stamp = rospy.Time.now()
map_msg.header.frame_id = self.map.frame
self.pub_merged_map.publish(map_msg)
@Timer(rospy.Duration(0.1))
def timer_callback(self, event):
if self.map is None:
rospy.logwarn("No map")
return
if self.goal is None:
rospy.logwarn("No goal")
return
try:
tf = self.tf_buffer.lookup_transform(
target_frame=self.map.frame,
source_frame=self.base_frame,
time=event.current_real,
timeout=rospy.Duration(0.1)
).transform
except (tf2_ros.LookupException, tf2_ros.ExtrapolationException) as e:
rospy.logwarn("TF error getting robot pose", exc_info=True)
return
# skip times when we are already off-map
curr = self.map[self.map.index_at(ros_numpy.numpify(tf.translation))]
if curr > 0 or curr is np.ma.masked:
rospy.logwarn("Current node is not valid")
return
# don't do anything if we requested single-shot planning
if self.replan == 'on_goal_change' and not self.goal_changed:
return
self.goal_changed = False
# determine our pose
if self.use_ackermann_rrt or self.use_ackermann_rrt2:
pose = model.pose_from_ros(tf)
else:
pose = ros_numpy.numpify(tf.translation)[:2]
# either reuse or throw out the RRT
if not self.rrt:
self.start_over(pose)
else:
if self.use_ackermann_rrt2:
pose_lookup = pose
elif self.use_ackermann_rrt:
pose_lookup = pose.xy
else:
pose_lookup = pose
# decide whether we can reroot the rrt
nearest, dist, _ = self.rrt.get_nearest_node(pose_lookup, exchanged=True)
if dist > self.reroute_threshold:
rospy.loginfo("Nearest node is {}m away, ignoring".format(dist))
self.start_over(pose)
else:
rospy.loginfo("Nearest node is {}m away, rerooting and pruning".format(dist))
nearest.make_root()
self.rrt.prune()
self.pub_vis_tree.publish(rrtutils.delete_marker_array_msg)
if self.do_profile:
# run and profile the rrt
pr = cProfile.Profile()
pr.enable()
path = list(self.rrt.run())
if self.do_profile:
pr.disable()
pr.dump_stats(os.path.join(
rospkg.RosPack().get_path('lab6'), 'rrt_stats.txt'
))
# process the results
self.send_debug_tree(path)
self.publish_path(path)
rospy.loginfo('Planned!')
def publish_path(self, edges):
# Send the path to the path_follower node
cmd = Path()
poses = np.concatenate([
edge.interpolate(step=0.2)[:-1]
for edge in edges
] + [[edges[-1].dest]])
cmd.poses = [
rrtutils.pose_for_node(pose) for pose in poses
]
cmd.header.frame_id = self.map.frame
#rospy.loginfo("New Poses {}.".format(cmd.poses))
self.pub_path.publish(cmd)
def send_debug_tree(self, path=[]):
"""
Renders the tree. Edges are red unless they appear in path, in which case they are green
"""
msg = MarkerArray()
for i, edge in enumerate(path):
marker = rrtutils.marker_for_edge(edge, id=i, frame=self.map.frame)
marker.color.r *= 0.5
marker.color.b *= 0.5
marker.color.g *= 0.75
marker.color.a = 1
marker.scale.x *= 2
msg.markers.append(marker)
self.pub_vis_tree.publish(msg)