class LoadTrajectory(object): """ Loads a trajectory from the file system and publishes it to a ROS topic. """ def __init__(self): self.path = rospy.get_param("~trajectory") self.should_publish = bool(rospy.get_param("~publish")) self.pub_topic = rospy.get_param("~topic") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) if self.should_publish: self.traj_pub = rospy.Publisher(self.pub_topic, PolygonStamped, queue_size=1) # need to wait a short period of time before publishing the first message time.sleep(0.5) # visualize the loaded trajectory for 5 seconds self.trajectory.publish_viz(duration=3.0) # send the trajectory if self.should_publish: self.publish_trajectory() def publish_trajectory(self): print "Publishing trajectory to:", self.pub_topic self.traj_pub.publish(self.trajectory.toPolygon())
def __init__(self): self.current_pose = None self.goal_pose = None # The map data, in row-major order, starting with (0,0). Occupancy # probabilities are in the range [0,100]. Unknown is -1. self.map_occupancy_prob = None self.obstacles = None # The origin of the map [m, m, rad]. This is the real-world pose of the # cell (0,0) in the map. self.map_origin_pose = None # Map width, height [cells] self.map_dimensions = (0, 0) # The map resolution [m/cell] self.map_res = None # MAKE SURE YOU UNCOMMENT THIS AND SET THE ODOM TOPIC RIGHT # self.odom_topic = rospy.get_param("/particle_filter/odom_topic", "/odom") self.trajectory = LineTrajectory("/planned_trajectory") self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry, self.odom_cb)
def __init__(self): # Define Adjustaable Parameters self.min_speed = rospy.get_param("~min_speed") self.max_speed = rospy.get_param("~max_speed") self.max_acceleration = rospy.get_param("~max_acceleration") self.max_decceleration = rospy.get_param("~max_decceleration") # Internal USE Variables - Do not modify without consultation self.dynamic_model = utils.ApproximateDynamicsModel() self.ackermann_model = utils.AckermannModel(0.36) self.trajectory = LineTrajectory("/speed_track") self.counts = 0 # Publishers self.viz_track_pub = rospy.Publisher("/speed_track/viz", MarkerArray, queue_size=1) self.traj_pub = rospy.Publisher(rospy.get_param("~pub_topic"), PolygonStamped, queue_size=1) # Subscriber self.traj_sub = rospy.Subscriber(rospy.get_param("~sub_topic"), String, self.trajCB, queue_size=1) print "Initialized. Waiting on messages..." # need to wait a short period of time before publishing the first message time.sleep(0.5)
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)
def __init__(self): self.path = rospy.get_param( "~trajectory", default= "/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories/2018-08-09-16-16-58.traj" ) self.should_publish = bool(rospy.get_param("~publish", default=True)) self.pub_topic = rospy.get_param( "~topic", default="/loaded_trajectory/recorded_path") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) if self.should_publish: self.traj_pub = rospy.Publisher(self.pub_topic, Path, queue_size=1) # need to wait a short period of time before publishing the first message time.sleep(0.5) # visualize the loaded trajectory for 5 seconds self.trajectory.publish_viz(duration=3.0) self.duration = rospy.Duration(1) self.times = 0 self.srv_get_raw_path_ = rospy.Service('/get_raw_path', GetRawPath, self.handle_get_raw_path) # send the trajectory if self.should_publish: self.publish_trajectory()
class LoadTrajectory(object): """ Loads a trajectory from the file system and publishes it to a ROS topic. """ def __init__(self): self.path = rospy.get_param("~trajectory") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) self.pub_topic = "/trajectory/current" self.traj_pub = rospy.Publisher(self.pub_topic, PoseArray, queue_size=1) # need to wait a short period of time before publishing the first message rospy.loginfo("waiting to publish trajectory") time.sleep(3.0) # visualize the loaded trajectory self.trajectory.publish_viz() # send the trajectory self.publish_trajectory() def publish_trajectory(self): print "Publishing trajectory to:", self.pub_topic self.traj_pub.publish(self.trajectory.toPoseArray())
class PathPlan(object): """ Listens for goal pose published by RViz and uses it to plan a path from current car pose. """ def __init__(self): self.odom_topic = rospy.get_param("~odom_topic") self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb) self.trajectory = LineTrajectory("/planned_trajectory") self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.odom_cb) def map_cb(self, msg): pass ## REMOVE AND FILL IN ## def odom_cb(self, msg): pass ## REMOVE AND FILL IN ## def goal_cb(self, msg): pass ## REMOVE AND FILL IN ## def plan_path(self, start_point, end_point, map): ## CODE FOR PATH PLANNING ## # publish trajectory self.traj_pub.publish(self.trajectory.toPoseArray()) # visualize trajectory Markers self.trajectory.publish_viz()
def __init__(self): # FIELDS #self.goal_pos = None #self.start_pos = None #self.g_map = None #self.map_x_offset = None #self.map_y_offset = None #self.map_resolution = None # Euler angnles: #self.yaw = 0 #self.pitch = 0 #self.roll = 0 #self.CCW_rotation_matrix = [] #self.CW_rotation_matrix = [] # SUBSCRIBERS and PUBLISHERS #self.odom_topic = rospy.get_param("~odom_topic") self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb) self.trajectory = LineTrajectory("/planned_trajectory") self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.odom_sub = rospy.Subscriber("/tesse/odom", Odometry, self.odom_cb)
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)
def __init__(self): self.odom_topic = rospy.get_param("~odom_topic") self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb) self.trajectory = LineTrajectory("/planned_trajectory") self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.odom_cb)
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 __init__(self): rospack = rospkg.RosPack() lab6_path = rospack.get_path("lab6") self.save_path = os.path.join(lab6_path+"/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)
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)
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 __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.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 # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory)
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 __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 __init__(self): self.path = rospy.get_param("~trajectory") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) self.pub_topic = "/trajectory/current" self.traj_pub = rospy.Publisher(self.pub_topic, PoseArray, queue_size=1) # need to wait a short period of time before publishing the first message time.sleep(0.5) # visualize the loaded trajectory self.trajectory.publish_viz() # send the trajectory self.publish_trajectory()
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)
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)
def __init__(self): # Define Adjustable Parameters self.stop_pt = Point(float(rospy.get_param("~stop_pt_x")), float(rospy.get_param("~stop_pt_y")), 0) self.check_pt = Point(float(rospy.get_param("~check_pt_x")), float(rospy.get_param("~check_pt_y")), 0) self.finish_pt = Point(float(rospy.get_param("~finish_pt_x")), float(rospy.get_param("~finish_pt_y")), 0) self.heading_min = float(rospy.get_param("~heading_min")) self.heading_max = float(rospy.get_param("~heading_max")) self.charging_route = rospy.get_param("~charging_route") self.charge_stop_voltage = int(rospy.get_param("~charge_stop_voltage")) self.charge_stop_offset = int(rospy.get_param("~charge_stop_offset")) # Internal Use Variables - Do not modify without consultation self.trajectory = LineTrajectory("/charging_trajectory") self.init = False self.step_counter = 0 self.first_time = True self.contactor_counter = 0 self.finish_charging_counter = 0 self.mag_data = [] self.mag_end = False self.cnt = 0 self.voltage_percent = 0 self.spike_cnt = 0 self.charge_OK = False self.iters = 0 # Subscribers self.init_charging_sub = rospy.Subscriber(rospy.get_param("~init_charging_topic"), Bool, self.init_chargingCB, queue_size=1) self.battery_sub = rospy.Subscriber(rospy.get_param("~battery_topic"), String, self.batteryCB, queue_size=1) self.pose_sub = rospy.Subscriber(rospy.get_param("~pose_topic"), Odometry, self.poseCB, queue_size=1) self.magnetic_sub = rospy.Subscriber(rospy.get_param("~magnetic_topic"), String, self.magCB, queue_size=1) # Publishers self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1) self.mag_drive_pub = rospy.Publisher(rospy.get_param("~mag_drive_topic"), Twist, queue_size=1) self.contactor_pub = rospy.Publisher(rospy.get_param("~contactor_topic"), Bool, queue_size=1) self.stop_charging_pub = rospy.Publisher(rospy.get_param("~stop_charging_topic"), Bool, queue_size=1) self.finish_charging_pub = rospy.Publisher(rospy.get_param("~finish_charging_topic"), Bool, queue_size=1) self.traj_pub = rospy.Publisher("/charging_navi/trajectory", PolygonStamped, queue_size=1) self.suspend_pub = rospy.Publisher(rospy.get_param("~suspend_topic"), Bool, queue_size=1) self.notify_pub = rospy.Publisher(rospy.get_param("~notify_topic"), String, queue_size=1) self.init_turn_pub = rospy.Publisher("/turning/init", String, queue_size=1)
def __init__(self): # Define Adjustable Parameters # - Scan Region (Rectangular) self.x_min = float(rospy.get_param("~x_min")) # [m] self.x_max = float(rospy.get_param("~x_max")) # [m] self.y_min = float(rospy.get_param("~y_min")) # [m] self.y_max = float(rospy.get_param("~y_max")) # [m] # - Table Size self.table_width = float(rospy.get_param("~table_width")) # [m] self.table_length = float(rospy.get_param("~table_length")) # [m] self.table_diagonal = float(rospy.get_param("~table_diagonal")) # [m] self.table_tolerance = float(rospy.get_param("~table_tolerance")) # [m] # - Route Trim self.route_trim_x = rospy.get_param("~route_trim_x") # [m] self.route_trim_y = rospy.get_param("~route_trim_y") # [m] # Internal Use Variables - Do not modify without consultation self.init = False # Need to be "False" when integrated with others self.cluster_tolerance = 0.1 # [m] self.table = False self.trajectory = LineTrajectory("/table_trajectory") self.tfTL = TransformListener() self.pathpoint_1 = None self.pathpoint_2 = None self.refresh_rate = rospy.Rate(10) self.stop_counter = 0 self.centerpoint = None # Subscribers self.scan_sub = rospy.Subscriber(rospy.get_param("~scan_topic"), LaserScan, self.scanCB, queue_size=1) self.pose_sub = rospy.Subscriber(rospy.get_param("~pose_topic"), Odometry, self.poseCB, queue_size=1) self.init_table_sub = rospy.Subscriber(rospy.get_param("~init_table_topic"), Bool, self.init_tableCB, queue_size=1) # Publishers self.route_pub = rospy.Publisher(rospy.get_param("~route_topic"), PolygonStamped, queue_size=1) self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1) self.finish_table_pub = rospy.Publisher(rospy.get_param("~finish_table_topic"), Bool, queue_size=1) self.viz_table_pub = rospy.Publisher(rospy.get_param("~viz_table_topic"), Marker, queue_size=1) self.viz_points_pub = rospy.Publisher(rospy.get_param("~viz_points_topic"), Marker, queue_size=1) # Publish Route for Navigating Under Table self.route_publisher()
class TrajectoryCaller(object): """ Call a pre-defined trajectory from the file system and publishes it to a ROS topic. """ def __init__(self): # Define Topics for Publishing and Subscribing Messages self.sub_topic = rospy.get_param("~sub_topic", "trajectory/new") self.pub_topic = rospy.get_param("~pub_topic", "trajectory/current") # Internal Use Variables - Do not modify without consultation self.counts = 0 # Initialize and Load the trajectory self.trajectory = LineTrajectory("/called_trajectory") # Subscriber self.traj_sub = rospy.Subscriber(self.sub_topic, String, self.trajCB, queue_size=1) # Publisher self.traj_pub = rospy.Publisher(self.pub_topic, PolygonStamped, queue_size=1) # Callback Function for receiving path def trajCB(self, msg): path_id = msg.data if self.counts == 1: self.publish_trajectory(path_id) self.counts = 0 self.counts = self.counts + 1 # Republish the trajectory after being called def publish_trajectory(self, path_name): # clear the trajectory whenever going to load new path self.trajectory.clear() # load a new received path self.trajectory.load(path_name) print "Publishing trajectory to:", self.pub_topic self.traj_pub.publish(self.trajectory.toPolygon())
def __init__(self): # Define Topics for Publishing and Subscribing Messages self.sub_topic = rospy.get_param("~sub_topic", "trajectory/new") self.pub_topic = rospy.get_param("~pub_topic", "trajectory/current") # Internal Use Variables - Do not modify without consultation self.counts = 0 # Initialize and Load the trajectory self.trajectory = LineTrajectory("/called_trajectory") # Subscriber self.traj_sub = rospy.Subscriber(self.sub_topic, String, self.trajCB, queue_size=1) # Publisher self.traj_pub = rospy.Publisher(self.pub_topic, PolygonStamped, queue_size=1)
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 LoadTrajectory(object): """ Loads a trajectory from the file system and publishes it to a ROS topic. """ def __init__(self): self.path = rospy.get_param( "~trajectory", default= "/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories/2018-08-09-16-16-58.traj" ) self.should_publish = bool(rospy.get_param("~publish", default=True)) self.pub_topic = rospy.get_param( "~topic", default="/loaded_trajectory/recorded_path") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) if self.should_publish: self.traj_pub = rospy.Publisher(self.pub_topic, Path, queue_size=1) # need to wait a short period of time before publishing the first message time.sleep(0.5) # visualize the loaded trajectory for 5 seconds self.trajectory.publish_viz(duration=3.0) self.duration = rospy.Duration(1) self.times = 0 self.srv_get_raw_path_ = rospy.Service('/get_raw_path', GetRawPath, self.handle_get_raw_path) # send the trajectory if self.should_publish: self.publish_trajectory() def publish_trajectory(self): print "Publishing trajectory to:", self.pub_topic while not rospy.is_shutdown(): if self.traj_pub.get_num_connections() > 0: self.traj_pub.publish(self.trajectory.toPath()) time.sleep(self.duration.to_sec()) self.times = self.times + 1 if self.times > 20: break def handle_get_raw_path(self, req): if not isinstance(self.trajectory, LineTrajectory): self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) return GetRawPathResponse(self.trajectory.toPath())
def __init__(self): self.odom_topic = rospy.get_param("~odom_topic") self.map_sub = rospy.Subscriber("/obstacles", OccupancyGrid, self.obstacle_map_cb) self.trajectory = LineTrajectory("/final_trajectory") self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_cb, queue_size=10) self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/final", PoseArray, queue_size=10) self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.odom_cb, queue_size=10) self.rest_of_trajectory = LineTrajectory("/rest_of_trajectory") self.planning_dist = 50.0 # FOR TESTING # self.vis_pub = rospy.Publisher("/particles", Marker, queue_size=1) self.nearest_pub = rospy.Publisher("/nearest", Marker, queue_size=1) self.start_pose = None self.goal_pose = None #self.max_iterations = 1000 # Max size of RRT* tree before breaking. self.dim = 2 # Configuration space dimensionality ([x, y] -> 2, [x, y, theta] -> 3) self.gamma = 10 # I don't actually know what this is called lol, it's used in rrt_star.radius() self.eta = .75 self.goal_bias = 0.15 self.map = None # FOR TESTING PURPOSES ONLY -- set to true if you want to see the tree grow self.enable_vis = True
def __init__(self): self.path = rospy.get_param("~trajectory") self.should_publish = bool(rospy.get_param("~publish")) self.pub_topic = rospy.get_param("~topic") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) if self.should_publish: self.traj_pub = rospy.Publisher(self.pub_topic, PolygonStamped, queue_size=1) # need to wait a short period of time before publishing the first message time.sleep(0.5) # visualize the loaded trajectory for 5 seconds self.trajectory.publish_viz(duration=3.0) # send the trajectory if self.should_publish: self.publish_trajectory()
class LoadTrajectory(object): """ Loads a trajectory from the file system and publishes it to a ROS topic. """ def __init__(self): self.path = rospy.get_param( "~trajectory", default= "/home/lenovo/zh/workspace/catkin_ws/src/ddl_0725/localization_0725/trajectories/2018-08-09-16-16-58.traj" ) self.should_publish = bool(rospy.get_param("~publish", default=True)) self.pub_topic = rospy.get_param( "~topic", default="/loaded_trajectory/recorded_path") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) if self.should_publish: self.traj_pub = rospy.Publisher(self.pub_topic, Path, queue_size=1) # need to wait a short period of time before publishing the first message time.sleep(0.5) # visualize the loaded trajectory for 5 seconds self.trajectory.publish_viz(duration=3.0) self.duration = rospy.Duration(1) # send the trajectory if self.should_publish: self.publish_trajectory() def publish_trajectory(self): print "Publishing trajectory to:", self.pub_topic while not rospy.is_shutdown(): self.traj_pub.publish(self.trajectory.toPath()) time.sleep(self.duration.to_sec())
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)