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...'
