def __init__(self): # parameters self.ANGLE_STEP = int(rospy.get_param("~angle_step")) self.MAX_PARTICLES = int(rospy.get_param("~max_particles")) self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) self.INV_SQUASH_FACTOR = 1.0 / float(rospy.get_param("~squash_factor")) self.MAX_RANGE_METERS = float(rospy.get_param("~max_range")) self.THETA_DISCRETIZATION = int(rospy.get_param("~theta_discretization")) self.WHICH_RM = rospy.get_param("~range_method", "cddt").lower() self.RANGELIB_VAR = int(rospy.get_param("~rangelib_variant", "3")) self.SHOW_FINE_TIMING = bool(rospy.get_param("~fine_timing", "0")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.DO_VIZ = bool(rospy.get_param("~viz")) self.PUBLISH_MAP_TO_ODOM = bool(rospy.get_param("~publish_map_to_odom", "0")) self.TRANSFORM_TOLERANCE = float(rospy.get_param("~transform_tolerance", 0.5)) # sensor model constants self.Z_SHORT = float(rospy.get_param("~z_short", 0.01)) self.Z_MAX = float(rospy.get_param("~z_max", 0.07)) self.Z_RAND = float(rospy.get_param("~z_rand", 0.12)) self.Z_HIT = float(rospy.get_param("~z_hit", 0.75)) self.SIGMA_HIT = float(rospy.get_param("~sigma_hit", 8.0)) # motion model constants self.MOTION_DISPERSION_X = float(rospy.get_param("~motion_dispersion_x", 0.05)) self.MOTION_DISPERSION_Y = float(rospy.get_param("~motion_dispersion_y", 0.025)) self.MOTION_DISPERSION_THETA = float(rospy.get_param("~motion_dispersion_theta", 0.25)) # various data containers used in the MCL algorithm self.MAX_RANGE_PX = None self.odometry_data = np.array([0.0,0.0,0.0]) self.laser = None self.iters = 0 self.map_info = None self.map_initialized = False self.lidar_initialized = False self.odom_initialized = False self.last_pose = None self.laser_angles = None self.downsampled_angles = None self.range_method = None self.last_time = None self.last_stamp = None self.first_sensor_update = True self.state_lock = Lock() # cache this to avoid memory allocation in motion model self.local_deltas = np.zeros((self.MAX_PARTICLES, 3)) # cache this for the sensor model computation self.queries = None self.ranges = None self.tiled_angles = None self.sensor_model_table = None # particle poses and weights self.inferred_pose = None self.particle_indices = np.arange(self.MAX_PARTICLES) self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) self.get_omap() self.precompute_sensor_model() self.initialize_global() # these topics are for visualization self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size = 1) self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size = 1) self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan", LaserScan, queue_size = 1) self.rect_pub = rospy.Publisher("/pf/viz/poly1", PolygonStamped, queue_size = 1) if self.PUBLISH_ODOM: self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size = 1) # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() if self.PUBLISH_MAP_TO_ODOM: self.tf_listener = tf.TransformListener() # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param("~scan_topic", "/scan"), LaserScan, self.lidarCB, queue_size=1) self.odom_sub = rospy.Subscriber(rospy.get_param("~odometry_topic", "/odom"), Odometry, self.odomCB, queue_size=1) self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) print "Finished initializing, waiting on messages..."
def __init__(self): # Get parameters self.particle_filter_frame = rospy.get_param("~particle_filter_frame") self.MAX_PARTICLES = int(rospy.get_param("~num_particles")) self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.ANGLE_STEP = int(rospy.get_param("~angle_step")) self.DO_VIZ = bool(rospy.get_param("~do_viz")) # MCL algorithm self.iters = 0 self.lidar_initialized = False self.odom_initialized = False self.map_initialized = False self.last_odom_pose = None # last received odom pose self.last_stamp = None self.laser_angles = None self.downsampled_angles = None self.state_lock = Lock() # paritcles self.inferred_pose = None self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.particle_indices = np.arange(self.MAX_PARTICLES) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel() # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) self.initialize_global() # map self.permissible_region = None self.SHOW_FINE_TIMING = False # these topics are for visualization self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size=1) self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size=1) self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan", LaserScan, queue_size=1) self.path_pub = rospy.Publisher('/pf/viz/path', Path, queue_size=1) self.path = Path() if self.PUBLISH_ODOM: self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size=1) # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param( "~scan_topic", "/scan"), LaserScan, self.callback_lidar, queue_size=1) self.odom_sub = rospy.Subscriber(rospy.get_param( "~odom_topic", "/odom"), Odometry, self.callback_odom, queue_size=1) self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) print "Finished initializing, waiting on messages..."
def __init__(self): rospy.loginfo('Trajectory topic: %s' % self.TRAJECTORY_TOPIC) # Publishers and subscribers. self.traj_sub = rospy.Subscriber(self.TRAJECTORY_TOPIC, PolygonStamped, self.trajectory_callback, queue_size=10) self.local_sub = rospy.Subscriber(self.LOCALIZATION_TOPIC, PoseStamped, self.localization_callback, queue_size=10) self.drive_pub = rospy.Publisher(self.DRIVE_TOPIC, AckermannDriveStamped, queue_size=10) self.test_pub = rospy.Publisher('/test_data', String, queue_size=10) self.traj_pub = rospy.Publisher('/pure_pursuit/subsampled', Marker, queue_size=10) # Boolean to signal that a trajectory has been received. self.has_traj = False # List of 2D points along the received trajectory. self.traj_pts = [] # A list of speeds to command between each pair of waypoints. self.traj_speeds = [] # Once the controller seeks the final endpoint, the path is complete. self.seeking_end_pt = False # Store a circular buffer of drive commands for the derivative term. self.steer_command_buffer = utils.CircularArray(10) for _ in range(10): self.steer_command_buffer.append(0) # Init curr pose of vehicle within map frame. self.curr_pose = (0, 0) self.curr_angle = 0 self.goal_pose = (0, 0) # Initialize goal pose (set later). # Stores index of current pt in the trajectory that vehicle is aiming for. self.curr_traj_goal_pt = 0 # Initialize drive msg. self.drive_msg = AckermannDriveStamped() self.drive_msg.header.frame_id = "base_link" self.drive_msg.drive.steering_angle = 0.0 self.drive_msg.drive.speed = 0.0 # Don't limit the steering vel and acceleration of the vehicle. self.drive_msg.drive.steering_angle_velocity = 0 self.drive_msg.drive.acceleration = 0 self.drive_msg.drive.jerk = 0 self.start_time = 0.0 self.end_time = 0.0 self.prev_angle_comms = [None] * 10 rospy.loginfo('Initialized Pure Pursuit node!')
def __init__(self): # parameters self.ANGLE_STEP = int(rospy.get_param("~angle_step")) self.MAX_PARTICLES = int(rospy.get_param("~max_particles")) self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) self.INV_SQUASH_FACTOR = 1.0 / float(rospy.get_param("~squash_factor")) self.MAX_RANGE_METERS = float(rospy.get_param("~max_range")) self.THETA_DISCRETIZATION = int( rospy.get_param("~theta_discretization")) self.WHICH_RM = rospy.get_param("~range_method", "cddt").lower() self.RANGELIB_VAR = int(rospy.get_param("~rangelib_variant", "3")) self.SHOW_FINE_TIMING = bool(rospy.get_param("~fine_timing", "0")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.DO_VIZ = bool(rospy.get_param("~viz")) # Mapping params (namespaced under /mapping). self.USE_LOCAL_MAP = Utils.getParamOrFail("/mapping/use_local_map") self.MAP_BASE_PATH = Utils.getParamOrFail("/mapping/map_base_path") self.LOCAL_MAP_XLIM = Utils.getParamOrFail("/mapping/local_map_xlim") self.LOCAL_MAP_YLIM = Utils.getParamOrFail("/mapping/local_map_ylim") self.OCCUPANCY_INC_PER_SCAN = Utils.getParamOrFail( "/mapping/occupancy_inc_per_scan") self.OCCUPANCY_DECAY_PER_SCAN = Utils.getParamOrFail( "/mapping/occupancy_decay_per_scan") self.DILATION_RADIUS_METERS = Utils.getParamOrFail( "/mapping/dilation_radius_meters") self.MAX_RANGE_METERS = Utils.getParamOrFail( "/mapping/max_range_meters") self.MIN_RANGE_METERS = Utils.getParamOrFail( "/mapping/min_range_meters") self.LOCAL_MAP_RESOLUTION = Utils.getParamOrFail( "/mapping/local_map_resolution") # various data containers used in the MCL algorithm self.MAX_RANGE_PX = None self.odometry_data = np.array([0.0, 0.0, 0.0]) self.laser = None self.iters = 0 self.map_msg = None self.map_info = None self.map_initialized = False self.lidar_initialized = False self.odom_initialized = False self.last_pose = None self.laser_angles = None self.downsampled_angles = None self.range_method = None self.last_time = None self.last_stamp = None self.first_sensor_update = True self.state_lock = Lock() # cache this to avoid memory allocation in motion model self.local_deltas = np.zeros((self.MAX_PARTICLES, 3)) # cache this for the sensor model computation self.queries = None self.ranges = None self.tiled_angles = None self.sensor_model_table = None # particle poses and weights self.inferred_pose = None self.particle_indices = np.arange(self.MAX_PARTICLES) self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) self.dynamic_map = None self.base_map_mask = None self.get_omap() self.precompute_sensor_model() self.initialize_global() ###################### MAP SETUP ###################### kernel_width = int(self.DILATION_RADIUS_METERS / 0.0504) self.dilate_kernel = np.ones((kernel_width, kernel_width)) localMapWidth = int((self.LOCAL_MAP_XLIM[1] - self.LOCAL_MAP_XLIM[0]) / self.LOCAL_MAP_RESOLUTION) localMapHeight = int( (self.LOCAL_MAP_YLIM[1] - self.LOCAL_MAP_YLIM[0]) / self.LOCAL_MAP_RESOLUTION) self.local_map_origin_pixel = np.array([ (0 - self.LOCAL_MAP_XLIM[0]) / self.LOCAL_MAP_RESOLUTION, (0 - self.LOCAL_MAP_YLIM[0]) / self.LOCAL_MAP_RESOLUTION ]).astype(int) # This map is a uint8 so that OpenCV functions can be used. self.local_map = np.zeros((localMapHeight, localMapWidth, 1), np.uint8) # Set up a local map message once. self.local_map_dilated = np.zeros(self.local_map.shape) self.local_map_msg = OccupancyGrid() self.local_map_msg.header.frame_id = '/laser' self.local_map_msg.info.resolution = self.LOCAL_MAP_RESOLUTION self.local_map_msg.info.width = localMapWidth self.local_map_msg.info.height = localMapHeight self.local_map_msg.info.origin.position.x = self.LOCAL_MAP_XLIM[0] self.local_map_msg.info.origin.position.y = self.LOCAL_MAP_YLIM[0] self.coords_disc = None self.map_debug_pub = rospy.Publisher('/map_debug/', OccupancyGrid, queue_size=1) self.dilated_map_pub = rospy.Publisher('/dilated_map', OccupancyGrid, queue_size=1) self.local_map_pub = rospy.Publisher('/local_map', OccupancyGrid, queue_size=1) ####################### END MAP SETUP #################### # these topics are for visualization self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size=10) self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size=1) self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan", LaserScan, queue_size=10) self.rect_pub = rospy.Publisher("/pf/viz/poly1", PolygonStamped, queue_size=1) if self.PUBLISH_ODOM: self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size=1) # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() self.cartesian_debug = rospy.Publisher("/cartesian_debug", Marker, queue_size=1) # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param( "~scan_topic", "/scan"), LaserScan, self.lidarCB, queue_size=1) self.odom_sub = rospy.Subscriber(rospy.get_param( "~odometry_topic", "/odom"), Odometry, self.odomCB, queue_size=1) self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) # self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) # self.map_debug_pub = rospy.Publisher('/map_debug/', OccupancyGrid, queue_size=5) # self.dilated_map_pub = rospy.Publisher('/dilated_map', OccupancyGrid, queue_size=10) self.clicked = False self.clicked_time = 0 self.SETTLE_TIME = 1 print "Finished initializing, waiting on messages..."
def __init__(self): # parameters self.ANGLE_STEP = int(rospy.get_param("~angle_step")) self.MAX_PARTICLES = int(rospy.get_param("~max_particles")) self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) self.INV_SQUASH_FACTOR = 1.0 / float(rospy.get_param("~squash_factor")) self.MAX_RANGE_METERS = float(rospy.get_param("~max_range")) self.THETA_DISCRETIZATION = int(rospy.get_param("~theta_discretization")) self.WHICH_RM = rospy.get_param("~range_method", "cddt").lower() self.RANGELIB_VAR = int(rospy.get_param("~rangelib_variant", "3")) self.SHOW_FINE_TIMING = bool(rospy.get_param("~fine_timing", "0")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.DO_VIZ = bool(rospy.get_param("~viz")) # various data containers used in the MCL algorithm self.MAX_RANGE_PX = None self.odometry_data = np.array([0.0,0.0,0.0]) self.laser = None self.iters = 0 self.map_info = None self.map_initialized = False self.lidar_initialized = False self.odom_initialized = False self.last_pose = None self.laser_angles = None self.downsampled_angles = None self.range_method = None self.last_time = None self.last_stamp = None self.first_sensor_update = True self.goal_set = False self.last_inferred_pose = None self.pose_settled = False self.init_pose = False self.state_lock = Lock() self.m_to_ft = 3.28084 self.avoid_dist = float(rospy.get_param("avoid_dist")) # variables for checking if cone is in acceptable region self.map2worldScale = 0.0504 self.world2mapScale = 1/float(self.map2worldScale) self.w2mRotation = np.array([-1, 1]) * self.world2mapScale self.w2mTranslation = np.array([25.9,16.5]) * self.world2mapScale cone_map = imread("/home/racecar/racecar-ws/src/race_0/maps/cone_map_flap.png") cone_map_dilated = imread("/home/racecar/racecar-ws/src/race_0/maps/cone_map_dilated.png") self.acceptable_cone_coordinates = load_map(cone_map_dilated) self.acceptable_waypoint_coordinates = load_map(cone_map) # cache this to avoid memory allocation in motion model self.local_deltas = np.zeros((self.MAX_PARTICLES, 3)) # cache this for the sensor model computation self.queries = None self.ranges = None self.tiled_angles = None self.sensor_model_table = None # particle poses and weights self.inferred_pose = None self.particle_indices = np.arange(self.MAX_PARTICLES) self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) self.get_omap() self.precompute_sensor_model() self.initialize_global() # these topics are for visualization self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size = 1) self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size = 1) self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan", LaserScan, queue_size = 1) self.rect_pub = rospy.Publisher("/pf/viz/poly1", PolygonStamped, queue_size = 1) self.viz_namespace = "/cone_detect" self.cone_pub = rospy.Publisher(self.viz_namespace + "/found_cone", Marker, queue_size = 1) self.cone_waypoint_pub1 = rospy.Publisher(self.viz_namespace + "/filter_waypoint1", Marker, queue_size = 1) self.cone_waypoint_pub2 = rospy.Publisher(self.viz_namespace + "/filter_waypoint2", Marker, queue_size = 1) # self.map_goal_pub = rospy.Publisher("/pf/viz/goal_point",PoseStamped, queue_size=1) # topic for path planner self.map_pose_pub = rospy.Publisher("/infered_pose_map", PoseStamped, queue_size = 1) self.map_goal_pub = rospy.Publisher("/goal_point",PoseStamped, queue_size=1) # oublishing cones as points self.cone_location_pub = rospy.Publisher("/cone_location", Float32MultiArray, queue_size = 1) if self.PUBLISH_ODOM: self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size = 1) # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param("~scan_topic", "/scan"), LaserScan, self.lidarCB, queue_size=1) self.odom_sub = rospy.Subscriber(rospy.get_param("~odometry_topic", "/odom"), Odometry, self.odomCB, queue_size=1) self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) self.cone_sub = rospy.Subscriber("pursuit_control_input",Float32MultiArray, self.cone_viz2, queue_size=1) # self.goal_sub = rospy.Subscriber("/goal_point_click", PoseStamped, self.clicked_pose, queue_size=1) self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.clicked_pose, queue_size=1) print "In Lab 6" print "Finished initializing, waiting on messages..."
def __init__(self): # parameters self.ANGLE_STEP = int(rospy.get_param('/{}/particle_filter/angle_step'.format(car_name))) self.MAX_PARTICLES = int(rospy.get_param('/{}/particle_filter/max_particles'.format(car_name))) self.MAX_VIZ_PARTICLES = int(rospy.get_param('/{}/particle_filter/max_viz_particles'.format(car_name))) self.INV_SQUASH_FACTOR = 1.0 / float(rospy.get_param('/{}/particle_filter/squash_factor'.format(car_name))) self.MAX_RANGE_METERS = float(rospy.get_param('/{}/particle_filter/max_range'.format(car_name))) self.THETA_DISCRETIZATION = int(rospy.get_param('/{}/particle_filter/theta_discretization'.format(car_name))) self.WHICH_RM = rospy.get_param('/{}/particle_filter/range_method'.format(car_name), 'cddt').lower() self.RANGELIB_VAR = int(rospy.get_param('/{}/particle_filter/rangelib_variant'.format(car_name), '3')) self.SHOW_FINE_TIMING = bool(rospy.get_param('/{}/particle_filter/fine_timing'.format(car_name), '0')) self.PUBLISH_ODOM = bool(rospy.get_param('/{}/particle_filter/publish_odom'.format(car_name), '1')) self.DO_VIZ = bool(rospy.get_param('/{}/particle_filter/viz'.format(car_name))) # sensor model constants self.Z_SHORT = float(rospy.get_param('/{}/particle_filter/z_short'.format(car_name), 0.01)) self.Z_MAX = float(rospy.get_param('/{}/particle_filter/z_max'.format(car_name), 0.07)) self.Z_RAND = float(rospy.get_param('/{}/particle_filter/z_rand'.format(car_name), 0.12)) self.Z_HIT = float(rospy.get_param('/{}/particle_filter/z_hit'.format(car_name), 0.75)) self.SIGMA_HIT = float(rospy.get_param('/{}/particle_filter/sigma_hit'.format(car_name), 8.0)) # motion model constants self.MOTION_DISPERSION_X = float(rospy.get_param('/{}/particle_filter/motion_dispersion_x'.format(car_name), 0.05)) self.MOTION_DISPERSION_Y = float(rospy.get_param('/{}/particle_filter/motion_dispersion_y'.format(car_name), 0.025)) self.MOTION_DISPERSION_THETA = float(rospy.get_param('/{}/particle_filter/motion_dispersion_theta'.format(car_name), 0.25)) # various data containers used in the MCL algorithm self.MAX_RANGE_PX = None self.odometry_data = np.array([0.0,0.0,0.0]) self.laser = None self.iters = 0 self.map_info = None self.map_initialized = False self.lidar_initialized = False self.odom_initialized = False self.last_pose = None self.laser_angles = None self.downsampled_angles = None self.range_method = None self.last_time = None self.last_stamp = None self.first_sensor_update = True self.state_lock = Lock() # cache this to avoid memory allocation in motion model self.local_deltas = np.zeros((self.MAX_PARTICLES, 3)) # cache this for the sensor model computation self.queries = None self.ranges = None self.tiled_angles = None self.sensor_model_table = None # particle poses and weights self.inferred_pose = None self.particle_indices = np.arange(self.MAX_PARTICLES) self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) self.get_omap() self.precompute_sensor_model() self.initialize_global() # these topics are for visualization self.pose_pub = rospy.Publisher('/{}/particle_filter/viz/inferred_pose'.format(car_name), PoseStamped, queue_size = 1) self.particle_pub = rospy.Publisher('/{}/particle_filter/viz/particles'.format(car_name), PoseArray, queue_size = 1) self.pub_fake_scan = rospy.Publisher('/{}/particle_filter/viz/fake_scan'.format(car_name), LaserScan, queue_size = 1) self.rect_pub = rospy.Publisher('/{}/particle_filter/viz/poly1'.format(car_name), PolygonStamped, queue_size = 1) if self.PUBLISH_ODOM: self.odom_pub = rospy.Publisher('/{}/odom_filtered'.format(car_name), Odometry, queue_size = 1) # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param('/{}/particle_filter/scan_topic', '/{}/scan'.format(car_name)), LaserScan, self.lidarCB, queue_size = 1) self.odom_sub = rospy.Subscriber(rospy.get_param('/{}/particle_filter/odometry_topic', '/{}/odom'.format(car_name)), Odometry, self.odomCB, queue_size = 1) # self.pose_sub = rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, self.clicked_pose, queue_size = 1) # self.click_sub = rospy.Subscriber('/clicked_point', PointStamped, self.clicked_pose, queue_size = 1) initial_pose_x = float(rospy.get_param('/{}/particle_filter/initial_pose_x'.format(car_name), '0.0')) initial_pose_y = float(rospy.get_param('/{}/particle_filter/initial_pose_y'.format(car_name), '0.0')) initial_pose_a = float(rospy.get_param('/{}/particle_filter/initial_pose_a'.format(car_name), '0.0')) initial_pose = Pose() initial_pose.position.x = initial_pose_x initial_pose.position.y = initial_pose_y initial_pose.orientation = Utils.yaw_to_quaternion(initial_pose_a) self.clicked_pose(initial_pose) print 'Finished initializing, waiting on messages...'