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!')