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 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))
def sub_pose(self, pose):
rospy.loginfo("notrrt: received goal")
trans = self.tf_buffer.lookup_transform(
target_frame="map",
source_frame="base_link",
time=pose.header.stamp,
timeout=rospy.Duration(1)
)
at = model.pose_from_ros(trans)
points = [(at.x,at.y),(pose.pose.position.x, pose.pose.position.y)]
rospy.loginfo("making path between: {}".format(points))
self.path = make_path(points)
self.goal_changed = True
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")
def sub_robot_path_update(self, msg):
'''
Every time our higher level logic gives us a new path to follow, we should replace our current planned path with the new path.
We should also assume that our next waypoint is now the first point on the new path.
'''
rospy.loginfo('Recieved a path')
# Assert that we have received a TF before trying to take on a path.
if self.robot_pose is None:
rospy.loginfo('Ignoring path since path_follower has not yet received a TF.')
return
self.path_frame = msg.header.frame_id
self.current_path = np.recarray(len(msg.poses), dtype=model.pose_dtype)
for i in range(len(msg.poses)):
p = msg.poses[i]
self.current_path[i] = model.pose_from_ros(p)
self.next_waypoint_index = 0
# Update navigation
self.update_navigation()
def sub_robot_pose_update(self, event):
'''
Every time the particle filter updates our current position, we should first check if we have reached a waypoint in our path. Then, we should tell the PID controller that our position and targets have changed.
'''
try:
tf = self.tf_buffer.lookup_transform(
target_frame=self.path_frame,
source_frame=self.base_frame,
time=event.current_real,
timeout=rospy.Duration(0.1)
).transform
except (tf2_ros.LookupException, tf2_ros.ExtrapolationException, tf2_ros.ConnectivityException) as e:
rospy.logwarn("Error getting robot position", exc_info=True)
return
self.robot_pose = model.pose_from_ros(tf)
# we have no path yet to give us a frame to refer to
if self.path_frame is None:
rospy.loginfo('No path yet')
else:
# Update navigation
self.update_navigation()
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!')