class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join( rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") # receive either goal points or clicked points self.publish_point_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_point_callback, queue_size=1) self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.clicked_point_callback, queue_size=1) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def clicked_point_callback(self, msg): if isinstance(msg, PointStamped): self.trajectory.addPoint(msg.point) elif isinstance(msg, PoseStamped): self.trajectory.addPoint(msg.pose.position) # publish visualization of the path being built self.trajectory.publish_viz() def saveTrajectory(self): self.trajectory.save(self.save_path)
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join( rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") ''' Insert appropriate subscribers/publishers here ''' self.point_listener = rospy.Subscriber("/clicked_point", PointStamped, self.point_cb, queue_size=1) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def point_cb(self, clicked_point): self.trajectory.addPoint(clicked_point.point) self.trajectory.publish_start_point() self.trajectory.publish_end_point() self.trajectory.publish_trajectory() def saveTrajectory(self): self.trajectory.save(self.save_path)
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join("trajectories/", time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") #%Y-%m-%d-%H-%M-%S self.trajectory = LineTrajectory("/built_trajectory") self.data_points = [] self.count = 0 self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20) self.trajectory.publish_viz() # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def publish_trajectory(self): self.traj_pub.publish(self.trajectory.toPoseArray()) def saveTrajectory(self): self.trajectory.save(self.save_path) def clicked_pose(self, msg): self.count += 1 point = Point() point.x = msg.point.x point.y = msg.point.y self.trajectory.addPoint(point) self.data_points.append(point) self.mark_pt(self.trajectory_points, (0, 1, 0), self.data_points) if self.count > 2: rospy.loginfo("PUBLISH TRAJ") self.publish_trajectory() self.trajectory.publish_viz() def mark_pt(self, subscriber, color_tup, data): mark_pt = Marker() mark_pt.header.frame_id = "/map" mark_pt.header.stamp = rospy.Time.now() mark_pt.type = mark_pt.SPHERE_LIST mark_pt.action = mark_pt.ADD mark_pt.scale.x = .5 mark_pt.scale.y = .5 mark_pt.scale.z = .5 mark_pt.color.a = 1.0 mark_pt.color.r = color_tup[0] mark_pt.color.g = color_tup[1] mark_pt.color.b = color_tup[2] mark_pt.points = data subscriber.publish(mark_pt)
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join( rospy.get_param( "~save_path", default= "/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories" ), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") self.odom_topic = rospy.get_param("~odom_topic", default="/pf/odom") self.sub_odom = rospy.Subscriber(self.odom_topic, Odometry, self.odomCB, queue_size=2) self.sub_pose = rospy.Subscriber("/current_pose", PoseStamped, self.poseCB, queue_size=1) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def odomCB(self, msg): self.trajectory.addPoint( msg.pose.pose.position.x, msg.pose.pose.position.y, quaternion_to_angle(msg.pose.pose.orientation)) self.trajectory.publish_viz() def poseCB(self, msg): self.trajectory.addPoint(msg.pose.position.x, msg.pose.position.y, quaternion_to_angle(msg.pose.orientation)) self.trajectory.publish_viz() def saveTrajectory(self): self.trajectory.save(self.save_path)
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join( rospy.get_param( "~save_path", default= "/home/lenovo/zh/workspace/catkin_ws/src/ddl_0725/localization_0725/trajectories" ), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") self.odom_topic = rospy.get_param("~odom_topic", default="/pf/odom") self.sub_odom = rospy.Subscriber(self.odom_topic, Odometry, self.odomCB, queue_size=2) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def clicked_point_callback(self, msg): if isinstance(msg, PointStamped): self.trajectory.addPoint(msg.point) elif isinstance(msg, PoseStamped): self.trajectory.addPoint(msg.pose.position) # publish visualization of the path being built self.trajectory.publish_viz() def odomCB(self, msg): self.trajectory.addPoint( msg.pose.pose.position.x, msg.pose.pose.position.y, quaternion_to_angle(msg.pose.pose.orientation)) self.trajectory.publish_viz() def saveTrajectory(self): self.trajectory.save(self.save_path)
class PathBuilder(object): def __init__(self): self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry, self.odom_callback) self.traj = LineTrajectory() rospack = rospkg.RosPack() fc_path = rospack.get_path("final_challenge") self.save_path = os.path.join( fc_path + "/trajectories/", time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") rospy.on_shutdown(self.traj.save(self.save_path)) def odom_callback(self, msg): point = msg.pose.pose.position #get Point from Odometry msg self.traj.addPoint(point)
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join(rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") #%Y-%m-%d-%H-%M-%S self.trajectory = LineTrajectory("/built_trajectory") ''' Insert appropriate subscribers/publishers here ''' self.data_points = [] self.count = 0 self.num_waypoints = 4 self.waypoints = [] self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PolygonStamped, queue_size=10) self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20) self.trajectory.publish_viz() #duration=40.0 self.rtt_pub = rospy.Publisher("/rtt/final", Marker, queue_size = 10) self.rtt_tree_pub = rospy.Publisher("/rtt/tree", Marker, queue_size = 10000) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) # Create a simulated laser scan self.scan_sim = PyScanSimulator2D( 2, # number of beams to send out 0, # field of view centered around theta = 0 0, # don't add noise 0.01, # used as an epsilon 500) # discretize the theta space for faster ray tracing #subscribe to map self.map_set = False self.permissible_region = None self.resolution = None self.origin = None self.width = None self.height = None self.permissible_indices = None rospy.Subscriber( "/map", OccupancyGrid, self.map_callback, queue_size = 1) self.rrt = RRT(self.rtt_tree_pub,self.scan_sim) def map_callback(self,map_msg): """ #convert the map to a numpy array print('HI') self.resolution = map_msg.info.resolution self.width = map_msg.info.width self.height = map_msg.info.height #ccmap_ = np.array(map_msg.data, np.double)/100 #map_ = np.clip(map_,0,1) #print(map_.shape) """ # assign map-based attributes self.resolution = map_msg.info.resolution self.width = map_msg.info.width self.height = map_msg.info.height # Convert the origin to a tuple origin_p = map_msg.info.origin.position origin_o = map_msg.info.origin.orientation origin_o = tf.transformations.euler_from_quaternion(( origin_o.x, origin_o.y, origin_o.z, origin_o.w)) self.origin = (origin_p.x, origin_p.y, origin_o[2]) # 0: permissible, -1: unmapped, 100: blocked array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width)) # 0: not permissible, 1: permissible self.permissible_region = np.zeros_like(array_255, dtype = bool) self.permissible_region[array_255 == 0] = 1 # dilate the maps self.permissible_region = 1 - binary_dilation(1 - self.permissible_region, square(15)) scan_map = 1 - np.array(self.permissible_region.reshape((self.permissible_region.size,)), np.double) # Initialize a map with the laser scan self.scan_sim.set_map( scan_map, map_msg.info.height, map_msg.info.width, map_msg.info.resolution, self.origin, 0.5) # Consider anything < 0.5 to be free self.permissible_indices = [(r, c) for r in xrange(self.height) for c in xrange(self.width) if self.permissible_region[r][c] == 1] self.map_set = True print("Map initialized") def publish_trajectory(self): self.traj_pub.publish(self.trajectory.toPolygon()) def saveTrajectory(self): self.trajectory.save(self.save_path) def clicked_pose(self,msg): """ self.trajectory.addPoint(point) self.data_points.append(point) self.mark_pt(self.trajectory_points, (0,1,0), self.data_points) if self.count > 2: rospy.loginfo("PUBLISH TRAJ") print("publish traj") self.publish_trajectory() """ point = Point() point.x = msg.point.x point.y = msg.point.y self.waypoints.append(point) self.mark_pt(self.trajectory_points, (0,0,1), [point]) self.count +=1 print("Point detected") print(self.count) print(self.waypoints) if self.count == self.num_waypoints: #do stuff self.data_points = [] start = self.waypoints[0] for pt in self.waypoints[1:]: rospy.loginfo("Building rrt") result = self.rrt.run(start,pt) self.data_points = self.data_points + result start = result[-1] print("Publishing") print(result) mark = Marker() mark.header.frame_id = "/map" mark.header.stamp = rospy.Time.now() mark.type = Marker.LINE_STRIP mark.ns = "final_rtt_path" mark.id = 9000 mark.scale.x = 0.5 mark.scale.y = 0.5 mark.scale.z = 0.5 mark.points = self.data_points mark.color = ColorRGBA(1,0,0,1) self.rtt_pub.publish(mark) print("published...") self.count = 0 self.waypoints = [] def mark_pt(self, subscriber, color_tup, data): mark_pt = Marker() mark_pt.header.frame_id = "/map" mark_pt.id = self.count mark_pt.header.stamp = rospy.Time.now() mark_pt.type = mark_pt.SPHERE_LIST mark_pt.action = mark_pt.ADD mark_pt.scale.x = 1.0 mark_pt.scale.y = 1.0 mark_pt.scale.z= 1.0 mark_pt.color.a =1.0 mark_pt.color.r=color_tup[0] mark_pt.color.g = color_tup[1] mark_pt.color.b = color_tup[2] mark_pt.points = data subscriber.publish(mark_pt)
def handle_smooth_path(self, req): ''' 1. Save raw path to raw_path_. 2. Smooth path by gradient descending. 3. Remove point from smoothed path which is too close to its neighborhood points ,or its neighborhood points is too close(which means there is probably a peak in path). 4. Publish the result path ''' raw_path = req.raw_path tolerance = self.param_tolerance_ max_iterations = self.param_iterations_ alpha = self.param_alpha_ beta = self.param_beta_ margin = self.param_margin_ min_point_dist = self.param_min_point_dist_ if not isinstance(self.map_data_, OccupancyGrid): print 'Received smooth path request, but cannot smooth when map data not received.' return diff = tolerance + 1 step = 0 np_path = self.makeNpArray(raw_path) if not isinstance(np_path, object): return new_path = deepcopy(np_path) while step < max_iterations: if diff < tolerance: print 'smooth ok' break step += 1 diff = 0. pre_path = deepcopy(new_path) i = 1 while i != new_path.shape[0] - 2: new_path[i] += alpha * (pre_path[i] - new_path[i]) + beta * ( new_path[i - 1] + new_path[i + 1] - 2 * new_path[i]) if self.isCollision(new_path[i], self.map_data_, margin): new_path[i] = deepcopy(pre_path[i]) i += 1 continue if np.sum( (new_path[i - 1] - new_path[i + 1])**2) < min_point_dist: new_path = np.delete(new_path, i, axis=0) pre_path = np.delete(pre_path, i, axis=0) i -= 1 i += 1 diff += np.sum((new_path - pre_path)**2) print 'round: ', step, '; diff: ', diff, '; origin # of points: ', len( req.raw_path.poses ), '; result # of points: ', new_path.shape[ 0], '; # of deleted points: ', np_path.shape[0] - new_path.shape[0] smoothed_path = Path() smoothed_path.header = make_header(self.param_map_frame_) for i in xrange(new_path.shape[0]): pose = PoseStamped() pose.pose.position.x = new_path[i][0] pose.pose.position.y = new_path[i][1] pose.pose.position.z = 0 pose.pose.orientation = angle_to_quaternion(0) smoothed_path.poses.append(pose) resp = SmoothPathResponse(smoothed_path) if self.param_save: trajectory = LineTrajectory("/bulid_trajectory") trajectory.fromPath(smoothed_path) trajectory.save(self.param_save_path) return resp