def __init__(self,
odom_lin_sigma=0.025,
odom_ang_sigma=np.deg2rad(2),
meas_rng_noise=0.2,
meas_ang_noise=np.deg2rad(10)):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Publishers
self.pub_lines = rospy.Publisher("lines", Marker)
self.pub_particles = rospy.Publisher("particles", PoseArray)
self.pub_big_particle = rospy.Publisher("mean_particle", PoseStamped)
self.pub_odom = rospy.Publisher("mean_particle_odom", Odometry)
# Subscribers
self.sub_scan = rospy.Subscriber("scan", LaserScan,
self.laser_callback)
self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
# TF
self.tfBroad = tf.TransformBroadcaster()
# Incremental odometry
self.last_odom = None
self.odom = None
# Flags
self.new_odom = False
self.new_laser = False
self.pub = False
# Particle filter
self.part_filter = ParticleFilter(get_map(), 500, odom_lin_sigma,
odom_ang_sigma, meas_rng_noise,
meas_ang_noise)
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.pdTheta = pd_controller.PDController(500, 40, -200, 200)
self.pidTheta = pid_controller.PIDController(200,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.map = lab8_map.Map("lab8_map.json")
self.base_speed = 100
self.odometry = odometry.Odometry()
self.particle_filter = ParticleFilter(
map=self.map,
virtual_create=self.virtual_create,
num_particles=100,
distance=STRAIGHT_DISTANCE,
sigma_sensor=0.1,
sigma_theta=0.05,
sigma_distance=0.01,
)
_ = self.create.sim_get_position()
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab8_map.Map("lab8_map.json")
self.odometry = Odometry()
self.pidTheta = PIDController(300,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
# constants
self.base_speed = 100
self.variance_sensor = 0.1
self.variance_distance = 0.01
self.variance_direction = 0.05
self.world_width = 3.0 # the x
self.world_height = 3.0 # the y
self.filter = ParticleFilter(self.virtual_create,
self.variance_sensor,
self.variance_distance,
self.variance_direction,
num_particles=100,
world_width=self.world_width,
world_height=self.world_height)
def initParticleFilter(self,PFtype): self.pf = ParticleFilter().getParticleFilterClass(PFtype) #PFtype = "IMUPF" or "IMUPF2" self.pf.setParameter(0.01 ,0.001) #パーティクルフィルタのパラメータ（ノイズの分散） variance of noise self.M = 100 # パーティクルの数 num of particles self.X = [] # パーティクルセット set of particles self.loglikelihood = 0.0 self.count = 0
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("Coplanarity")
self.pf.setFocus(self.f)
self.pf.setParameter(self.PFnoise_a_sys, self.PFnoise_g_sys, self.PFnoise_a_obs, self.PFnoise_g_obs, self.PFnoise_coplanarity_obs) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 0
def __init__(self):
self.node_name = "tj2_romi_pf"
rospy.init_node(self.node_name,
# disable_signals=True
# log_level=rospy.DEBUG
)
# rospy.on_shutdown(self.shutdown_hook)
self.map_frame = rospy.get_param("~map_frame", "map")
self.base_frame = rospy.get_param("~base_frame", "base_link")
self.map_service_name = rospy.get_param("~static_map", "static_map")
self.show_particles = rospy.get_param("~publish_particles", True)
self.initial_range = rospy.get_param("~initial_range", None)
self.input_std = rospy.get_param("~input_std", None)
self.meas_std_val = rospy.get_param("~meas_std_val", 0.007)
self.num_particles = rospy.get_param("~num_particles", 100)
self.input_std = rospy.get_param("~input_std", None)
if self.initial_range is None:
self.initial_range = [1.0, 1.0, 1.0]
if self.input_std is None:
self.input_std = [0.007, 0.007]
self.input_vector = np.zeros(len(self.input_std))
self.pf = ParticleFilter(self.num_particles, self.meas_std_val,
self.input_std)
self.prev_pf_time = rospy.Time.now().to_sec()
self.map_raytracer = MapRaytracer(self.base_frame, "ultrasonic1",
"ultrasonic2")
self.ultrasonic1_sub = rospy.Subscriber("ultrasonic1",
Float64,
self.ultrasonic1_callback,
queue_size=25)
self.ultrasonic2_sub = rospy.Subscriber("ultrasonic2",
Float64,
self.ultrasonic2_callback,
queue_size=25)
self.odom_sub = rospy.Subscriber("odom",
Odometry,
self.odom_callback,
queue_size=25)
self.particles_pub = rospy.Publisher("pf_particles",
PoseArray,
queue_size=5)
self.broadcaster = tf2_ros.TransformBroadcaster()
self.get_map = rospy.ServiceProxy(self.map_service_name, GetMap)
rospy.loginfo("Waiting for service %s" % self.map_service_name)
rospy.wait_for_service(self.map_service_name)
self.map_msg = self.get_map().map
self.map_raytracer.set_map(self.map_msg)
rospy.loginfo("%s init done" % self.node_name)
def __init__(self, *args, **kwargs):
super(DemoWindow, self).__init__(*args, **kwargs)
self.graphWidget = pg.PlotWidget()
self.graphWidget.getViewBox().invertY(True)
self.graphWidget.getViewBox().invertX(True)
self.setCentralWidget(self.graphWidget)
self.graphWidget.showGrid(x=True, y=True)
self.graphWidget.setXRange(-2, 0)
self.graphWidget.setYRange(0, 2)
self.graphWidget.addLegend()
# initialize attributes
self.u = [0, 0]
self.dt = 0.1 # 10 hz updates
self.state = [-0.5, 1.5, 0]
self.est_state = self.state
# initialize filter
N = 50
wall_lengths = [2, 2]
# self.sensor_locations = ([0,-1], [1,-1], [1,0], [1,1], [0,1])
self.sensor_locations = ([1, 0], [1, -1], [0, -1], [-1, -1], [-1, 0],
[-1, 1], [0, 1], [1, 1])
# self.sensor_readings = [0, 0, 0, 0, 0]
self.sensor_readings = [0, 0, 0, 0, 0, 0, 0, 0]
sensor_max = [50, 50, 50, 50, 50]
initial_x = [-0.5, 1.5, 0]
self.filter = ParticleFilter(N,
sensor_locations=self.sensor_locations,
sensor_max=sensor_max,
wall_lengths=wall_lengths,
initial_x=initial_x)
# initialize ROS
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("cmd_vel", Twist, self.callback)
# setup timer to run particle filter at 5 Hz
self.timer = QtCore.QTimer()
self.timer.setInterval(50)
self.timer.timeout.connect(self.timer_callback)
self.timer.start()
# plot data: x, y values
pen1 = pg.mkPen(color='b')
pen2 = pg.mkPen(color='r')
self.pos_plot = self.graphWidget.plot([], [],
pen=pen2,
symbol='o',
symbolSize=10,
symbolBrush=('r'))
self.est_plot = self.graphWidget.plot([], [],
pen=pen1,
symbol='o',
symbolSize=10,
symbolBrush=('b'))
def go_particle():
img = Image()
detect = Detection()
found = []
initialState = None
pf = None
histograms = []
detections = []
hist_ref = None
#Recoleccion
while(True):
img.get()
#Face de recoleccion de información/ Aprendizaje
if len(histograms) < 10:
print len(histograms)
#Detect faces
found = detect.faces(img.image)
if len(found) > 0:
hist_ref = img.getColorHistogram(found[0]).ravel()
histograms.append(hist_ref)
img.show_hist(hist_ref)
hist_ref=hist_ref/float(hist_ref.sum())
else: #Aprendizaje finalizado
#Iniciar filtro, utilizar el ultimo ROI/found como initialState
# print len(hist_ref)
if initialState is None:
x,y,w,h = found[0]
hist = img.getColorHistogram(found[0]).ravel()
dx = random.uniform(0,5)
dy = random.uniform(0,5)
dw = random.uniform(0,.5)
dh = random.uniform(0,.5)
initialState = np.array([x,y,w,h,dx,dy,dw,dh])
ceroState = np.random.uniform(0,img.size[1],8)
cov = np.cov(np.asarray([initialState,ceroState]).T)
pf = ParticleFilter(initialState,cov,hist_ref,10.,100)
u=np.zeros((100,4))
for rect in pf.states:
rect = rect[:4]
img.draw_roi([rect])
# pf.update(img)
else: #ya se ha inicializado el filtro ahora se busca actualizar y predecir
old=pf.states
pf.predict(img.size[0],img.size[1],u)
pf.update(img)
for rect in pf.states:
rect = rect[:4]
img.draw_roi([rect])
u=(old-pf.states)[:,-4:]
img.show()
if 0xFF & cv2.waitKey(5) == 27:
break
def pf(self, prior_var, number_particles, prior_Voc, prior_Rs, prior_Rp,
prior_Cp, prior_Vc, observation_noise):
self.test = ParticleFilter(prior_var=prior_var,
number_particles=number_particles,
prior_Voc=prior_Voc,
prior_Rs=prior_Rs,
prior_Rp=prior_Rp,
prior_Cp=prior_Cp,
prior_Vc=prior_Vc,
observation_noise=observation_noise)
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("RBPF")
self.pf.setFocus(self.f)
self.pf.setParameter(
self.noise_a_sys, self.noise_g_sys, self.noise_a_sys_camera,
self.noise_camera) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 1
self.step = 1
def __init__(self,
odom_lin_sigma=0.025,
odom_ang_sigma=np.deg2rad(2),
meas_rng_noise=0.2,
meas_ang_noise=np.deg2rad(10),
x_init=0,
y_init=0,
theta_init=0):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Threads
self.lock = threading.RLock()
# Publishers
self.pub_map = rospy.Publisher("map", Marker, queue_size=2)
self.pub_lines = rospy.Publisher("lines", Marker, queue_size=2)
self.pub_lines_mean = rospy.Publisher("lines_mean",
Marker,
queue_size=2)
self.pub_particles = rospy.Publisher("particles",
PoseArray,
queue_size=2)
self.pub_big_particle = rospy.Publisher("mean_particle",
PoseStamped,
queue_size=2)
self.pub_odom = rospy.Publisher("mean_particle_odom",
Odometry,
queue_size=2)
self.pub_wei = rospy.Publisher("weights", MarkerArray, queue_size=2)
# Subscribers
self.sub_scan = rospy.Subscriber("lines", Marker, self.laser_callback)
self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
# TF
self.tfBroad = tf.TransformBroadcaster()
# Incremental odometry
self.last_odom = None
self.odom = None
# Flags
self.new_odom = False
self.new_laser = False
self.pub = False
self.time = None
self.lines = None
# Particle filter
self.part_filter = ParticleFilter(get_dataset3_map(), 500,
odom_lin_sigma, odom_ang_sigma,
meas_rng_noise, meas_ang_noise,
x_init, y_init, theta_init)
def main():
rospy.init_node('ParticleFilter', anonymous=True)
PF_l = ParticleFilter(Np=300)
r = rospy.Rate(5)
while not rospy.is_shutdown():
r.sleep()
PF_l.pub()
rospy.spin()
def get_system_state():
saved_particles = get_db("particles")
pf = ParticleFilter(particles=saved_particles)
p.start('system_state_generation')
result = json.dumps({
'particles': sample_particles(saved_particles),
'router_distances': get_router_dist(),
'mean': pf.get_position(),
'std': pf.get_std()
})
p.pstop('system_state_generation')
return result
def get_system_state():
saved_particles = get_db("particles")
pf = ParticleFilter(particles=saved_particles)
p.start('system_state_generation')
result = json.dumps({
'particles' : sample_particles(saved_particles),
'router_distances' : get_router_dist(),
'mean' : pf.get_position(),
'std' : pf.get_std()
})
p.pstop('system_state_generation')
return result
def main():
cap = cv2.VideoCapture(0)
ret, frame = cap.read()
print("MAIN:", frame.shape)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
x, y = 240, 320
pf = ParticleFilter(x, y, frame, n_particles=500, square_size=50, dt=0.20)
alpha = 0.5
while (True):
ret, frame = cap.read()
orig = np.array(frame)
img = frame
norm_factor = 255.0 / np.sum(frame, axis=2)[:, :, np.newaxis]
frame = frame * norm_factor
frame = cv2.convertScaleAbs(frame)
frame = cv2.blur(frame, (5, 5))
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
x, y, sq_size, distrib, distrib_control = pf.next_state(frame)
p1 = (int(y - sq_size), int(x - sq_size))
p2 = (int(y + sq_size), int(x + sq_size))
# before resampling
for (x2, y2, scale2) in distrib_control:
x2 = int(x2)
y2 = int(y2)
cv2.circle(img, (y2, x2), 1, (255, 0, 0), thickness=10)
# after resampling
for (x1, y1, scale1) in distrib:
x1 = int(x1)
y1 = int(y1)
cv2.circle(img, (y1, x1), 1, (0, 0, 255), thickness=10)
cv2.rectangle(img, p1, p2, (0, 0, 255), thickness=5)
cv2.addWeighted(orig, alpha, img, 1 - alpha, 0, img)
create_legend(img, (40, 40), (40, 20))
cv2.imshow('frame', img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.odometry = odometry.Odometry()
self.particle_filter = ParticleFilter()
def __init__(self,
true_obs,
num_particles=20,
num_iterations=1,
a=0.99,
b=1.0,
c=1.23,
d=0.0,
learn_rate=0.0001,
**kwargs):
ParticleFilter.__init__(self, true_obs, num_particles, num_iterations,
a, b, c, learn_rate, **kwargs)
self.d = d
def _update_filters(self, scene_changed, points):
"""
Method used to update each indiividual particle filter, using the Hungarian approach
:param scene_changed: A boolean flag used to indicate if the scene has changed, True if so.
:param points: The points provided to associate with each filter
"""
if scene_changed:
# Changed scene, so reinitialise the kalman filters
self.filters = []
if not self.filters:
for point in points:
self.filters.append(ParticleFilter(500, point))
return points
else:
dists = np.empty((len(self.filters), len(points)))
for i, filter in enumerate(self.filters):
filter.predict()
dists[i] = np.linalg.norm(points - filter.estimate(), axis=1)
new_filters = []
try:
row, col = linear_sum_assignment(dists)
except ValueError:
# linear_sum can fail at times, we revert to old filters if so
return self.filters
for i in range(len(col)):
# Row filters, col points
if dists[row[i], col[i]] > 25:
pass
# Too far - new pf from this
new_filters.append(ParticleFilter(500, points[col[i]]))
else:
filter = self.filters[row[i]]
# Add the old filter to the new list
new_filters.append(filter)
filter.update(points[col[i]])
filter.resample()
# Let any non-updated filters decay
for filter in self.filters:
# Inefficient but works
if filter not in new_filters and not filter.update_none():
# Gets to live another day
new_filters.append(filter)
# Update to new filters
self.filters = new_filters
def __init__(self):
# CV bridge
self.bridge = CvBridge()
# Particle filter
global lane_width
self.particle_filter = ParticleFilter(1000, -lane_width / 2,
lane_width / 2, -np.pi / 8.,
np.pi / 8., lane_width)
self.prev_v = 0.
self.prev_omega = 0.
self.prev_time = rospy.Time.now()
self.steps_since_resample = 0
# Publisers
self.mask_pub = rospy.Publisher('/mask', Image, queue_size=1)
self.edges_image_pub = rospy.Publisher('/edges_image',
Image,
queue_size=1)
self.line_segments_image_pub = rospy.Publisher('/line_segments_image',
Image,
queue_size=1)
self.lanes_image_pub = rospy.Publisher('/lanes_image',
Image,
queue_size=1)
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# Image subscriber
self.image_sub = rospy.Subscriber('/duckiebot/camera_node/image/rect',
Image, self.image_callback)
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("Coplanarity")
self.pf.setFocus(self.f)
self.pf.setParameter(self.noise_x_sys, self.PFnoise_coplanarity_obs) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 0
def main(DEBUG=False):
m = Map(imagePath, scale=100, sampleResolution=26)
d = Drone(m)
pf = ParticleFilter(m, d, numParticles=1000)
pf.calculateLikelihood()
pf.drawParticles()
# embed()
finish = False
manualMoveIncrement = m.scale_percent
while not finish:
update = False
dp = (0, 0)
util.drawCircle(m.image, m.positionToPixel(d.pos))
m.show()
m.clearImage()
# cv.imshow('image', m.sample(d.pos, sample_resolution=sampleWidth))
key = cv.waitKey(0)
if (key == ord('q')):
finish = True
elif (key == ord('w')):
dp = (0, -manualMoveIncrement)
d.move(dp)
update = True
elif (key == ord('s')):
dp = (0, manualMoveIncrement)
d.move(dp)
update = True
elif (key == ord('a')):
dp = (-manualMoveIncrement, 0)
d.move(dp)
update = True
elif (key == ord('d')):
dp = (manualMoveIncrement, 0)
d.move(dp)
update = True
elif (key == 13): #enter
dp = d.generateRandomMovementVector_map()
d.move(dp)
update = True
else:
print("Unrecognized key")
if (DEBUG):
distance = sum([util.distance(p.pos, d.pos)
for p in pf.particles]) / len(pf.particles)
print("true: ", d.pos)
[
print("{:01.2f} : {:01.2f}".format(p.pos[0], p.pos[1]))
for p in pf.particles
]
print("distance: ", distance)
pf.fullUpdate(dp)
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("RBPF")
self.pf.setFocus(self.f)
self.pf.setParameter(self.noise_x_sys, self.noise_a_sys, self.noise_g_sys, self.noise_camera, self.noise_coplanarity, self.noise_x_sys_coefficient) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 1
self.step = 1
def run(self):
self.create.start()
self.create.safe()
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to visualize the pose of our estimated position
# where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
# self.virtual_create.set_pose((0.5, 0.5, 0.1), 0)
origin = (0.5, 0.5, 0.1, 0) # partical origin
noise = (0.01, 0.05, 0.1) # sd for (distance, theta, sonar)
self.odometry.x = origin[0]
self.odometry.y = origin[1]
self.pf = ParticleFilter(origin, noise, 1000, self.map)
self.virtual_create.set_point_cloud(self.pf.get_particle_list())
# This is an example on how to show particles
# the format is x,y,z,theta,x,y,z,theta,...
# data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0]
# self.virtual_create.set_point_cloud(data)
# This is an example on how to estimate the distance to a wall for the given
# map, assuming the robot is at (0, 0) and has heading math.pi
# print(self.map.closest_distance((0.5,0.5), 0))
# self.positioning()
# This is an example on how to detect that a button was pressed in V-REP
while True:
b = self.virtual_create.get_last_button()
if b == self.virtual_create.Button.MoveForward:
self.forward()
elif b == self.virtual_create.Button.TurnLeft:
self.turn_left()
elif b == self.virtual_create.Button.TurnRight:
self.turn_right()
elif b == self.virtual_create.Button.Sense:
self.sense()
self.time.sleep(0.01)
def __init__(self):
rospy.init_node('pf')
real_robot = False
# create instances of two helper objects that are provided to you
# as part of the project
self.particle_filter = ParticleFilter()
self.occupancy_field = OccupancyField()
self.TFHelper = TFHelper()
self.sensor_model = sensor_model = SensorModel(
model_noise_rate=0.5,
odometry_noise_rate=0.15,
world_model=self.occupancy_field,
TFHelper=self.TFHelper)
self.position_delta = None # Pose, delta from current to previous odometry reading
self.last_scan = None # list of ranges
self.odom = None # Pose, current odometry reading
self.x_y_spread = 0.3 # Spread constant for x-y initialization of particles
self.z_spread = 0.2 # Spread constant for z initialization of particles
self.n_particles = 150 # number of particles
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
rospy.Subscriber("odom", Odometry, self.update_position)
rospy.Subscriber("stable_scan", LaserScan, self.update_scan)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
def init_tracker(self, frame, bbox):
tracker_types = [
'BOOSTING', 'MIL', 'KCF', 'TLD', 'MEDIANFLOW', 'GOTURN'
]
tracker_type = tracker_types[0]
if tracker_type == tracker_types[0]:
self.tracker = cv2.TrackerBoosting_create()
if tracker_type == 'MIL':
self.tracker = cv2.TrackerMIL_create()
# if tracker_type == 'KCF':
# self.tracker = cv2.TrackerKCF_create()
if tracker_type == 'TLD':
self.tracker = cv2.TrackerTLD_create()
if tracker_type == 'MEDIANFLOW':
self.tracker = cv2.TrackerMedianFlow_create()
if tracker_type == 'GOTURN':
self.tracker = cv2.TrackerGOTURN_create()
# Initialize tracker with first frame and bounding box
ok = self.tracker.init(frame, bbox)
self.particle_filter = ParticleFilter()
self.particle_filter = ParticleFilter.init_particles(
self.particle_filter, region=bbox, particlesPerObject=100)
img = frame[int(bbox[1]):int(bbox[1] + bbox[3]),
int(bbox[0]):int(bbox[0] + bbox[2])]
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
self.histo = cv2.calcHist([hsv], [0, 1], None, [180, 256],
[0, 180, 0, 256])
self.points.append(
ParticleFilter.get_particle_center(self.particle_filter))
self.frame_number += 1
return self
def next_frame(self, frame):
ok, bbox = self.tracker.update(frame)
frameHSV = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
w = frame.shape[0]
h = frame.shape[1]
self.particle_filter.transition(w, h)
self.particle_filter.update_weight(frameHSV, self.histo)
self.particle_filter.normalize_weights()
self.particle_filter.resample()
self.points.append(
ParticleFilter.get_particle_center(self.particle_filter))
self.frame_number += 1
return ok, bbox
def generateDataPoint1(nP, resampleRate, numIterations, headless):
ret = []
m = Map(imagePath, scale=100, sampleResolution=26)
d = Drone(m)
pf = ParticleFilter(m, d, numParticles=nP, resampleRate=resampleRate)
pf.calculateLikelihood()
for i in range(numIterations):
dp = d.generateRandomMovementVector_map()
d.move(dp)
pf.fullUpdate(dp)
# ret.append(pf.averageParticleDistance(d.pos))
ret.append(pf.numParticlesClusteredAroundGroundTruth(d.pos))
if not headless:
m.clearImage()
pf.drawParticles()
util.drawCircle(m.image, m.positionToPixel(d.pos))
m.show()
key = cv.waitKey(0)
return ret
def main():
global particles
global recive_particles
rospy.init_node('particle_filter', anonymous = True)
PF_l = ParticleFilter(Np=200)
PI_t = np.random.randn(200,3)
fusion = RobotFusion(PF_l.particles, PI_t)
particles2fuse_cb = lambda x: particles2fuse(x,PF_l,fusion)
rospy.Subscriber('/particlecloud2fuse_in', PoseArray, particles2fuse_cb)
r = rospy.Rate(5)
time_last= rospy.Time.now()
while not rospy.is_shutdown():
time_now = rospy.Time.now()
r.sleep()
fusion.PI_s = PF_l.particles
PF_l.pub()
E_x = np.mean(fusion.PI_s,axis=0)
if time_now.to_sec() - time_last.to_sec() > 1:
PF_l.pub2fuse()
time_last = time_now
if recive_particles:
print "recived particles!"
fusion.PI_t = particles
d = np.linalg.norm(np.mean(particles[:,0:2],axis=0) - fusion.mu_t_hat)
print "d= ", d
if d < 0.7:
PF_l.weights = fusion.update_weight()
PF_l.resampling()
recive_particles = 0
fusion.robot_tracking()
fusion.send_transfered_particles()
rospy.spin()
def __init__(self, num_particles=100, plotting=False):
super(Robot, self).__init__()
self.found_food = False
self.carrying = 0
self.data = Data()
self.plotting = plotting
self.r = pykhepera.PyKhepera()
self.axel_l = 53.0 # self.axis length in mm
self.data.clear()
self.data.pose = utils.Particle(utils.home_pose.x,
utils.home_pose.y,
utils.home_pose.theta, 0.5)
self.ppose = utils.home_pose
self.particle_filter = ParticleFilter(num_particles,
self.data.pose, self.data)
self.data.load_calibration()
self.speed_command = (0,0)
if self.plotting:
self.init_plotting()
def main():
rospy.init_node('SLAM', anonymous = True)
PF = ParticleFilter(Np=100)
OG = occupancy_grid()
OG.update_map(scan = PF.scan.z,initialize = 1)
PF.get_map(OG)
r = rospy.Rate(5)
while not rospy.is_shutdown():
r.sleep()
if PF.i_MU[0] > 0.01 or PF.i_MU[1] > 0.01:
OG.update_map(X = np.mean(PF.particles),scan = PF.scan.z,initialize = 0)
PF.get_map(OG)
PF.i_MU = [0.0,0.0]
rospy.spin()
def __init__(self,
n_particles,
initial_measurements,
init_time,
deviations,
plot,
plotsequence,
plotafter=0):
self.plot_folder = plot_path + "/plot" + str(plotsequence)
if not os.path.exists(self.plot_folder):
os.makedirs(self.plot_folder)
if plot and wipe_dir:
self.plotafter = plotafter
files = glob.glob(self.plot_folder + "/*")
for f in files:
os.remove(f)
self.plot = plot #boolean
self.last_t = init_time
# to avoid modifying particle and weights while another thread such as maneuver negotiator's no_conflict is
# reading
self.mutex = Lock()
#get initial directions that the vehicle are travelling towards
self.travelling_directions = [
intersection.getTravellingDirection(x, y, theta, "")
for (x, y, theta, _) in initial_measurements
]
#initialize the particle filters
self.particle_filters = \
[ParticleFilter(self.travelling_directions, intersection, n_particles, m, deviations) \
for m in initial_measurements]
self.updateMostLikelyStates()
self.grantList = {
} #key = car id, value = [estimated time of finishing intersction, turn]
def __init__(self, odom_lin_sigma=0.025, odom_ang_sigma=np.deg2rad(2), meas_rng_noise=0.2, meas_ang_noise=np.deg2rad(10), x_init=0, y_init=0, theta_init=0):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Threads
self.lock = threading.RLock()
# Publishers
self.pub_map = rospy.Publisher("map", Marker, queue_size = 2)
self.pub_lines = rospy.Publisher("lines", Marker, queue_size = 2)
self.pub_lines_mean = rospy.Publisher("lines_mean", Marker, queue_size = 2)
self.pub_particles = rospy.Publisher("particles", PoseArray, queue_size = 2)
self.pub_big_particle = rospy.Publisher("mean_particle", PoseStamped, queue_size = 2)
self.pub_odom = rospy.Publisher("mean_particle_odom", Odometry, queue_size = 2)
self.pub_wei = rospy.Publisher("weights", MarkerArray, queue_size = 2)
# Subscribers
self.sub_scan = rospy.Subscriber("lines", Marker, self.laser_callback)
self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
# TF
self.tfBroad = tf.TransformBroadcaster()
# Incremental odometry
self.last_odom = None
self.odom = None
# Flags
self.new_odom = False
self.new_laser = False
self.pub = False
self.time = None
self.lines = None
self.i = 0
# Particle filter
self.part_filter = ParticleFilter(get_dataset3_map(), 500, odom_lin_sigma, odom_ang_sigma, meas_rng_noise, meas_ang_noise, x_init, y_init, theta_init)
def create_particle_filter(initial_position, Q, R, initial_img_filename, args):
initial_img = plt.imread(initial_img_filename)
initial_position = initial_position
frame_id, x, y, w, h, is_lost = initial_position
x = float(x)
y = float(y)
w = float(w)
h = float(h)
initial_state = None
if args.point_estimate:
initial_state = (x + w / 2, y + h / 2)
else:
if not args.global_tracking:
initial_state = (x, y, x + w, y + h)
# Initialize PF
pf = ParticleFilter(num_particles=args.num_particles,
R=R,
img_shape=initial_img.shape,
resample_mode=args.resample_mode,
initial_state=initial_state,
point_estimate=args.point_estimate)
return pf
def get():
saved_particles = get_db("particles")
pf = ParticleFilter(particles=saved_particles)
return 'mean: '+ str(pf.get_position()) + ', std: ' +str(pf.get_std())
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab10_map.Map("lab11.map")
self.odometry = Odometry()
def showParticles(self, particles):
self.virtual_create.set_point_cloud(particles)
def visualizePose(self,x,y,z,theta):
self.virtual_create.set_pose((x, y, z), theta)
def turnLeftOneSec(self):
self.create.drive_direct(50,-50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnLeft()
def redrawParticles(self):
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
def findAndGoOnClearPath(self):
#Find new angle to move at
foundNewAngle = False
foundStartTime = False
runTime = 0
#Turn robot until clear space is found
while not foundNewAngle:
#Turn robot
self.turnLeftOneSec()
self.redrawParticles()
distanceFromNearestObstacle = self.sonar.get_distance()
obstacleAhead = distanceFromNearestObstacle < 0.001
if not obstacleAhead:
if not foundStartTime:
foundStartTime = True
else:
runTime += 1
if foundStartTime and runTime >= 4:
foundNewAngle = True
elif obstacleAhead and runTime < 4:
foundStartTime = False
runTime = 0
#Now turn the other way
turnRightTime = int(runTime / 2)
for i in range(0, turnRightTime):
self.create.drive_direct(-50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnRight()
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
#Actually move forward
self.create.drive_direct(50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.MoveForward()
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to visualize the pose of our estimated position
# where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
#self.visualizePose(0.5, 0.5, 0.1, math.pi)
# This is an example on how to show particles
# the format is x,y,z,theta,x,y,z,theta,...
#data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0]
#self.showParticles(data)
# This is an example on how to estimate the distance to a wall for the given
# map, assuming the robot is at (0, 0) and has heading math.pi
#print(self.map.closest_distance((0.5,0.5), math.pi))
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
#This is an example on how to use PF
variances = [0.01,0.01,0.3]
self.pf = PF([self.map.bottom_left[0], self.map.top_right[0]],[self.map.bottom_left[1], self.map.top_right[1]],[0,2*math.pi], variances, self.map)
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
# This is an example on how to detect that a button was pressed in V-REP
bNum = 0
while True:
self.create.drive_direct(0,0)
self.state = self.create.update()
if self.state:
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
prevX = self.odometry.x
prevY = self.odometry.y
prevTheta = self.odometry.theta
#b = self.virtual_create.get_last_button()
if self.pf.isOneCluster():
break
if bNum % 2 == 0:
b = self.virtual_create.Button.Sense
elif bNum % 4 == 1:
b = self.virtual_create.Button.TurnLeft
elif bNum % 4 == 3:
b = self.virtual_create.Button.MoveForward
bNum += 1
if b == self.virtual_create.Button.MoveForward:
print("Forward pressed!")
self.findAndGoOnClearPath()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.TurnLeft:
print("Turn Left pressed!")
self.create.drive_direct(50,-50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnLeft()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.TurnRight:
print("Turn Right pressed!")
self.create.drive_direct(-50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnRight()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.Sense:
print("Sense pressed!")
dist = self.sonar.get_distance()
if dist:
self.pf.Sensing(dist)
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
self.time.sleep(0.01)
class StateCoplanarity:
def __init__(self):
# ----- Set parameters here! ----- #
self.M = 512 # total number of particles パーティクルの数
self.f = 924.1770935 # focus length of camera [px] カメラの焦点距離 [px]
# Kalman Filter
self.noise_x_sys = 0.015 # system noise of position (SD) 位置のシステムノイズ（標準偏差）
self.noise_a_sys = 0.1 # system noise of acceleration (SD) 加速度のシステムノイズ（標準偏差）
self.noise_g_sys = 0.01 # system noise of orientation (SD) 角度のシステムノイズ（標準偏差）
self.noise_a_obs = 0.001 # observation noise of acceleration (SD) 加速度の観測ノイズ（標準偏差）
self.noise_g_obs = 0.0001 # observation noise of orientation (SD) 角度の観測ノイズ（標準偏差）
# Particle Filter
self.PFnoise_coplanarity_obs = 0.01 # observation noise of coplanarity (SD) 共面条件の観測ノイズ（標準偏差）
# ----- Set parameters here! ----- #
self.init()
def init(self):
self.isFirstTimeIMU = True
self.isFirstTimeCamera = True
self.lock = False
self.step = 1
self.t = 0.0
self.t1 = 0.0
self.t_camera = 0.0
self.t1_camera = 0.0
self.accel1 = np.array([0.0, 0.0, 0.0])
self.accel2 = np.array([0.0, 0.0, 0.0])
self.accel3 = np.array([0.0, 0.0, 0.0])
self.initKalmanFilter()
self.initParticleFilter()
def initKalmanFilter(self):
self.mu = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0])
self.mu1 = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0])
self.sigma = np.zeros([12,12])
self.C = np.array([ [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]])
self.Q = np.diag([self.noise_x_sys**2,self.noise_x_sys**2,self.noise_x_sys**2,0.0,0.0,0.0,self.noise_a_sys**2,self.noise_a_sys**2,self.noise_a_sys**2,self.noise_g_sys**2,self.noise_g_sys**2,self.noise_g_sys**2]) # sys noise
self.R = np.diag([self.noise_a_obs**2,self.noise_a_obs**2,self.noise_a_obs**2,self.noise_g_obs**2,self.noise_g_obs**2,self.noise_g_obs**2]) # obs noise
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("Coplanarity")
self.pf.setFocus(self.f)
self.pf.setParameter(self.noise_x_sys, self.PFnoise_coplanarity_obs) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 0
"""
This method is called from Sensor class when new IMU sensor data are arrived.
time_ : time (sec)
accel : acceleration in global coordinates
ori : orientaion
"""
def setSensorData(self, time_, accel, ori):
# Count
self.count+=1
# If process is locked by Image Particle Filter, do nothing
if(self.lock):
print("locked")
return
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
if(self.isFirstTimeIMU):
#init mu
self.mu = np.array([0.0,0.0,0.0,
0.0,0.0,0.0,
accel[0],accel[1],accel[2],
ori[0],ori[1],ori[2]])
else:
#observation
Y = np.array([accel[0],accel[1],accel[2],
ori[0],ori[1],ori[2]])
dt2 = 0.5 * self.dt * self.dt
#dt3 = (1.0 / 6.0) * dt2 * self.dt
A = np.array([[1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0,0.0],
[0.0,1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0],
[0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0],
[0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]])
#Qt = np.diag([dt2,dt2,dt2,self.dt,self.dt,self.dt,1.0,1.0,1.0,self.dt,self.dt,self.dt])
#Q = Qt.dot(self.Q)
"""
self.accel3 = copy.deepcopy(self.accel2)
self.accel2 = copy.deepcopy(self.accel1)
self.accel1 = copy.deepcopy(accel)
if(Util.isDeviceMoving(self.accel1[0]) == False and Util.isDeviceMoving(self.accel2[0]) == False and Util.isDeviceMoving(self.accel3[0]) == False):
self.mu[3] = 0.0
if(Util.isDeviceMoving(self.accel1[1]) == False and Util.isDeviceMoving(self.accel2[1]) == False and Util.isDeviceMoving(self.accel3[1]) == False):
self.mu[4] = 0.0
if(Util.isDeviceMoving(self.accel1[2]) == False and Util.isDeviceMoving(self.accel2[2]) == False and Util.isDeviceMoving(self.accel3[2]) == False):
self.mu[5] = 0.0
"""
self.mu, self.sigma = KF.execKF1Simple(Y,self.mu,self.sigma,A,self.C,self.Q,self.R)
if(self.isFirstTimeIMU):
self.isFirstTimeIMU = False
"""
This method is called from Image class when new camera image data are arrived.
time_ : time (sec)
keypointPairs : list of KeyPointPair class objects
"""
def setImageData(self, time_, keypointPairs):
# If IMU data has not been arrived yet, do nothing
if(self.isFirstTimeIMU):
return
# Count
self.count+=1
########################
print("===================================")
print("step "+str(self.step))
###########################
# If first time, save mu and don't do anything else
if(self.isFirstTimeCamera):
self.isFirstTimeCamera = False
self.mu1 = copy.deepcopy(self.mu) # save mu[t] as mu[t-1]
self.t_camera = time_
self.step += 1
return
# Lock IMU process
self.lock = True
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.t1_camera = self.t_camera
self.t_camera = time_
dt_camera = self.t_camera - self.t1_camera
# create particle from state vector
self.X = self.createParticleFromStateVector(self.mu, self.sigma)
# create X1 from mu[t-1]
X1 = Particle()
X1.initWithMu(self.mu1)
self.saveXYZasCSV(self.X,"1") ##############################
# exec particle filter
self.X = self.pf.pf_step(self.X, X1, self.dt, dt_camera, keypointPairs, self.M)
self.saveXYZasCSV(self.X,"2") ##############################
# create state vector from particle set
self.mu, self.sigma = self.createStateVectorFromParticle(self.X)
# save mu[t] as mu[t-1]
self.mu1 = copy.deepcopy(self.mu)
# Step (camera only observation step)
self.step += 1
# Unlock IMU process
self.lock = False
"""
save (X,Y,Z) of particles as CSV file
"""
def saveXYZasCSV(self,X,appendix):
x = []
for X_ in X:
x.append(X_.x)
date = datetime.datetime.now()
#datestr = date.strftime("%Y%m%d_%H%M%S_") + "%04d" % (date.microsecond // 1000)
#np.savetxt('./data/plot3d/'+datestr+'_xyz_'+appendix+'.csv', x, delimiter=',')
datestr = date.strftime("%Y%m%d_%H%M%S_")
np.savetxt('./data/plot3d/'+datestr+str(self.count)+'_xyz_'+appendix+'.csv', x, delimiter=',')
"""
create particle set from state vector
"""
def createParticleFromStateVector(self, mu, sigma):
X = []
for i in range(self.M):
particle = Particle()
particle.initWithStateVector(mu, sigma)
X.append(particle)
return X
"""
create state vector from particle set
"""
def createStateVectorFromParticle(self, X):
x = []
v = []
a = []
o = []
for X_ in X:
x.append(X_.x)
v.append(X_.v)
a.append(X_.a)
o.append(X_.o)
x_mu = np.mean(x, axis=0)
v_mu = np.mean(v, axis=0)
a_mu = np.mean(a, axis=0)
o_mu = np.mean(o, axis=0)
x_var = np.var(x, axis=0)
v_var = np.var(v, axis=0)
a_var = np.var(a, axis=0)
o_var = np.var(o, axis=0)
mu = np.array([x_mu[0],x_mu[1],x_mu[2],v_mu[0],v_mu[1],v_mu[2],a_mu[0],a_mu[1],a_mu[2],o_mu[0],o_mu[1],o_mu[2]])
sigma = np.diag([x_var[0],x_var[1],x_var[2],v_var[0],v_var[1],v_var[2],a_var[0],a_var[1],a_var[2],o_var[0],o_var[1],o_var[2]])
return mu, sigma
"""
This method is called from "Main.py"
return estimated state vector
"""
def getState(self):
x = np.array([self.mu[0],self.mu[1],self.mu[2]])
v = np.array([self.mu[3],self.mu[4],self.mu[5]])
a = np.array([self.mu[6],self.mu[7],self.mu[8]])
o = np.array([self.mu[9],self.mu[10],self.mu[11]])
return x,v,a,o
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to visualize the pose of our estimated position
# where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
#self.visualizePose(0.5, 0.5, 0.1, math.pi)
# This is an example on how to show particles
# the format is x,y,z,theta,x,y,z,theta,...
#data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0]
#self.showParticles(data)
# This is an example on how to estimate the distance to a wall for the given
# map, assuming the robot is at (0, 0) and has heading math.pi
#print(self.map.closest_distance((0.5,0.5), math.pi))
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
#This is an example on how to use PF
variances = [0.01,0.01,0.3]
self.pf = PF([self.map.bottom_left[0], self.map.top_right[0]],[self.map.bottom_left[1], self.map.top_right[1]],[0,2*math.pi], variances, self.map)
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
# This is an example on how to detect that a button was pressed in V-REP
bNum = 0
while True:
self.create.drive_direct(0,0)
self.state = self.create.update()
if self.state:
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
prevX = self.odometry.x
prevY = self.odometry.y
prevTheta = self.odometry.theta
#b = self.virtual_create.get_last_button()
if self.pf.isOneCluster():
break
if bNum % 2 == 0:
b = self.virtual_create.Button.Sense
elif bNum % 4 == 1:
b = self.virtual_create.Button.TurnLeft
elif bNum % 4 == 3:
b = self.virtual_create.Button.MoveForward
bNum += 1
if b == self.virtual_create.Button.MoveForward:
print("Forward pressed!")
self.findAndGoOnClearPath()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.TurnLeft:
print("Turn Left pressed!")
self.create.drive_direct(50,-50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnLeft()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.TurnRight:
print("Turn Right pressed!")
self.create.drive_direct(-50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnRight()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.Sense:
print("Sense pressed!")
dist = self.sonar.get_distance()
if dist:
self.pf.Sensing(dist)
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
self.time.sleep(0.01)
class Robot(object):
'''This is the logic/execution module for the pykhepera robot. It handles the high level functions such as reloading the robot, controlling the robot, etc.
'''
def __init__(self, num_particles=100, plotting=False):
super(Robot, self).__init__()
self.found_food = False
self.carrying = 0
self.data = Data()
self.plotting = plotting
self.r = pykhepera.PyKhepera()
self.axel_l = 53.0 # self.axis length in mm
self.data.clear()
self.data.pose = utils.Particle(utils.home_pose.x,
utils.home_pose.y,
utils.home_pose.theta, 0.5)
self.ppose = utils.home_pose
self.particle_filter = ParticleFilter(num_particles,
self.data.pose, self.data)
self.data.load_calibration()
self.speed_command = (0,0)
if self.plotting:
self.init_plotting()
def init_plotting(self):
plt.ion()
self.fig = plt.figure(figsize=(16, 12))
self.ax = self.fig.add_subplot(1, 1, 1)
self.ax.axis([0,1469, 0, 962])
self.line, = self.ax.plot(self.data.x_positions, self.data.y_positions)
self.p_lines, = self.ax.plot(self.particle_filter.get_x(),
self.particle_filter.get_y(), 'r.')
self.ppose_lines, = self.ax.plot(self.ppose.x, self.ppose.y, 'go')
self.im = np.flipud(misc.imread('arena.bmp'))
plt.imshow(self.im, origin='lower')
self.plotting_thread = threading.Thread(target=self.update_plot)
self.plotting_thread.daemon = True
self.plotting_thread.start()
def update_data(self):
self.data.sensor_values = self.r.read_sensor_values()
wheel_speeds = self.r.read_wheel_speeds()
self.data.wheel_speeds = wheel_speeds
self.data.wheel_values = self.r.read_wheel_values()
def calibrate_min(self):
self.r.reset_wheel_counters()
self.update_data()
mins = self.data.sensor_values
maxs = mins
self.r.turn((5, -5))
print self.data.wheel_values
while self.data.wheel_values[0] < 2000:
self.update_data()
aux = self.data.sensor_values
for i, val in enumerate(aux):
if val > mins[i]:
mins[i] = val
else:
maxs[i] = val
self.r.turn((0, 0))
m = 0
for val in mins:
m += val
m = m/8
maxs_avg = sum((val in maxs))/len(maxs)
self.data.thresholds['max_ir_reading'] = m
self.data.thresholds['wall_max'] = m
self.data.thresholds['wall_min'] = maxs_avg
if m:
print 'calibration succesful: ', mins
fout = open('data_calibration.data', 'a')
fout.write('%d:%s' % (m, ''.join(str(mins))))
fout.write('\n')
fout.close()
def largest_reading(self, sensor_i, duration=1, sample_rate=.5):
self.update_data()
max_reading = self.data.sensor_values[sensor_i]
for i in range(int(duration/sample_rate)):
self.update_data()
if self.data.sensor_values[sensor_i] > max_reading:
max_reading = self.data.sensor_values[sensor_i]
print 'reading: ', self.data.sensor_values[sensor_i]
time.sleep(sample_rate)
print 'max_reading for %d: %d' % (sensor_i, max_reading)
return max_reading
def calibrate_distances(self, closest=2, furthest=4):
readings = []
for i in range(9):
aux = []
readings.append(aux)
self.update_data()
par = -1
for a in range(9):
self.r.set_values('G', [0, 0])
readings[6].append(self.largest_reading(6))
readings[7].append(self.largest_reading(7))
self.r.rotate(par * np.pi/2)
time.sleep(1)
readings[0].append(self.largest_reading(0))
self.r.rotate(par * np.pi/4)
time.sleep(0.3)
readings[1].append(self.largest_reading(1))
self.r.rotate(par * np.pi/4)
time.sleep(0.3)
readings[2].append(self.largest_reading(2))
readings[3].append(self.largest_reading(3))
self.r.rotate(par * np.pi/4)
time.sleep(0.3)
readings[4].append(self.largest_reading(4))
self.r.rotate(par * np.pi/4)
time.sleep(0.3)
readings[5].append(self.largest_reading(5))
self.r.set_values('C', [0, 0])
time.sleep(3)
self.r.travel(utils.to_wu(10))
time.sleep(1)
for i in range(8):
sensor = 'sensor'+str(i)
self.data.thresholds[sensor] = readings[i]
# print self.data.thresholds
# avg_max = sum(r[closest] for r in self.data.distance_thresholds)/len(self.data.distance_thresholds)
# avg_min = sum(r[furthest] for r in self.data.distance_thresholds)/len(self.data.distance_thresholds)
# self.data.thresholds['max_ir_reading'] = avg_max
# self.data.thresholds['wall_min'] = avg_min
self.data.save_calibration()
def update_plot(self):
while self.plotting:
self.line.set_xdata(self.data.x_positions)
self.line.set_ydata(self.data.y_positions)
self.p_lines.set_xdata(self.particle_filter.get_x())
self.p_lines.set_ydata(self.particle_filter.get_y())
self.ppose_lines.set_xdata(self.ppose.x)
self.ppose_lines.set_ydata(self.ppose.y)
time.sleep(.1)
def will_collide(self):
for val in self.data.sensor_values[1:5]:
if val > self.data.thresholds['max_ir_reading']:
return True
def led_state(self, number):
'''converts a number to binary and lights LED accordingly'''
if number > 3:
return
binary = bin(number)[2:]
if len(binary) == 1:
binary = '0' + binary
for i, bit in enumerate(binary):
self.r.led(i, bit)
def update(self, dt):
self.update_data()
self.data.pose = utils.update_pose(self.data.pose, self.data.wheel_speeds, dt)
self.data.x_positions.append(self.data.pose.x)
self.data.y_positions.append(self.data.pose.y)
self.data.theta = self.data.pose.theta
self.particle_filter.update(dt)
self.ppose = self.particle_filter.most_likely()
if self.plotting:
self.fig.canvas.draw()
def check_food(self):
if len(self.data.sensor_values) == 0:
return
for val in self.data.sensor_values:
if val < 300:
return
else:
self.found_food = True
self.carrying += 1
def flash_leds(self):
for i in range(4):
self.led_state(3)
self.led_state(0)
def run(self):
obj_avoid = behaviors.ObjAvoid(self.data)
# wall_follow = behaviors.WallFollow(self.data)
# go_home = behaviors.GoHome(self.data)
last_time = time.time()
time_up = False
self.update_data()
try:
while True:
current_time = time.time()
# if (not time_up and current_time - start_time > 5):
# time_up = True
# if self.r.state is not 0:
# self.r.state = 3
dt = (current_time - last_time)
last_time = time.time()
self.check_food()
if self.found_food:
print "fooooooood!!"
self.found_food = False
self.r.stop()
self.flash_leds()
self.update(dt)
speed = (0, 0)
vals = self.data.sensor_values
if self.r.state is 0:
self.led_state(0)
if self.will_collide():
speed = (0, 0)
self.r.state = 1
else:
if self.carrying:
speed = (5, 5)
else:
speed = (5, 5)
if self.r.state is 1:
self.led_state(1)
speed = obj_avoid.step()
# if speed == (5, 5):
# if time_up:
# self.r.state = 3
# else:
# self.r.state = 2
# wall_follow.prev_vals = vals
# speed = (0, 0)
elif self.r.state is 2:
self.led_state(2)
if self.will_collide():
self.r.state = 1
continue
if (vals[0] < self.data.thresholds['wall_min']
and vals[5] < self.data.thresholds['wall_min']):
#"No wall :( "
self.r.state = 0
continue
speed = wall_follow.step()
#print "Following :)"
elif self.r.state is 3:
print 'going home'
self.led_state(3)
self.r.stop()
suggestion = go_home.step()
rotation = suggestion['rotation']
print 'wheel values: ', self.data.wheel_values
print 'rotation: ', rotation
self.r.rotate(rotation)
time.sleep(2)
self.r.state = 0
if speed:
self.r.turn(speed)
self.speed_command = speed
except KeyboardInterrupt:
print 'killing and cleaning up'
self.r.purge_buffer()
self.led_state(0)
self.plotting = False
self.plotting_thread.join()
self.r.stop()
self.r.kill()
self.particle_filter.tear_down()
def reset_pf(self): self._pf = ParticleFilter(particleGeneratorGenerator(5),N=500,regenProb=0.2)
class Game(object):
def __init__(self):
self._robot = Robot((XMAX/2+100,YMAX/2-25),0)
#self._robot = particleGeneratorGenerator(0)()
self._keydown = defaultdict(bool)
self.reset_pf()
def reset_pf(self):
self._pf = ParticleFilter(particleGeneratorGenerator(5),N=500,regenProb=0.2)
def draw(self,surf):
#self._robot.sense(surf)
#self._robot.draw(surf,(255,255,0))
for i in range(len(self._pf._particles)):
p = self._pf._particles[i]
p.draw(surf,color=(255,0,0))
def evolve(self):
pause = True
ds = 0
dh = 0
if self._keydown[pygame.K_x]:
pause = False
if self._keydown[pygame.K_r]:
self.reset_pf()
pause = False
if self._keydown[pygame.K_UP]:
pause = False
ds = 2
elif self._keydown[pygame.K_DOWN]:
pause = False
ds = -2
if self._keydown[pygame.K_RIGHT]:
pause = False
dh = pi/32
elif self._keydown[pygame.K_LEFT]:
pause = False
dh = -pi/32
if not pause:
action = (ds,dh)
self._robot.evolve(action)
ranges = self._robot.sense()
def likelihood(p):
ranges_prime = p.sense()
ranges_diff = np.abs(ranges_prime - ranges)
rv = 1.0
for i in range(len(ranges_diff)):
#rv *= exp(-(ranges_diff[i]/SIGMA_RANGE)**2)*0.5 + 0.5
if (ranges_diff[i] / SIGMA_RANGE) > 2:
rv *= 0.3
elif (ranges_diff[i] / SIGMA_RANGE) > 2:
rv *= 0.5
elif (ranges_diff[i] / SIGMA_RANGE) > 1:
rv *= 0.9
return rv
if not self._keydown[pygame.K_h]:
#self._pf.iterate(action,likelihood)
self._pf._evolve(action)
if self._keydown[pygame.K_x]:
self._pf._update(likelihood)
def handle_event(self,event):
if event.type == pygame.KEYDOWN:
self._keydown[event.key] = True
elif event.type == pygame.KEYUP:
self._keydown[event.key] = False
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.pdTheta = pd_controller.PDController(500, 40, -200, 200)
self.pidTheta = pid_controller.PIDController(200,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.map = lab8_map.Map("lab8_map.json")
self.base_speed = 100
self.odometry = odometry.Odometry()
self.particle_filter = ParticleFilter(
map=self.map,
virtual_create=self.virtual_create,
num_particles=100,
distance=STRAIGHT_DISTANCE,
sigma_sensor=0.1,
sigma_theta=0.05,
sigma_distance=0.01,
)
_ = self.create.sim_get_position()
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to visualize the pose of our estimated position
# where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
# self.virtual_create.set_pose((0.5, 0.5, 0.1), 0)
# This is an example on how to show particles
# the format is x,y,z,theta,x,y,z,theta,...
# data = [0.5, 0.5, 0.1, math.pi / 2, 1.5, 1, 0.1, 0]
# self.virtual_create.set_point_cloud(data)
# This is an example on how to estimate the distance to a wall for the given
# map, assuming the robot is at (0, 0) and has heading math.pi
# print(self.map.closest_distance((0.5, 0.5), 0))
self.particle_filter.draw_particles()
# This is an example on how to detect that a button was pressed in V-REP
while True:
# self.particle_filter.draw_particles()
b = self.virtual_create.get_last_button()
if b == self.virtual_create.Button.MoveForward:
self.forward(STRAIGHT_DISTANCE)
self.particle_filter.movement(Command.straight, 0)
# print("Forward pressed!")
elif b == self.virtual_create.Button.TurnLeft:
desired_angle = math.pi / 2
# desired_angle %= 2 * math.pi
self.particle_filter.movement(Command.turn_left, desired_angle)
self.sleep(0.01)
self.turn_left()
# self.go_to_angle(self.odometry.theta+math.pi/2)
# print("Turn Left pressed!")
elif b == self.virtual_create.Button.TurnRight:
desired_angle = -math.pi / 2
# desired_angle %= 2 * math.pi
self.particle_filter.movement(Command.turn_right,
desired_angle)
self.sleep(0.01)
self.turn_right()
# self.go_to_angle(self.odometry.theta-math.pi/2)
# print("Turn Right pressed!")
elif b == self.virtual_create.Button.Sense:
distance = self.sonar.get_distance()
self.particle_filter.sensing(distance)
self.sleep(0.01)
# def go_to_angle(self, goal_theta):
# while math.fabs(math.atan2(
# math.sin(goal_theta - self.odometry.theta),
# math.cos(goal_theta - self.odometry.theta))) > 0.01:
# output_theta = self.pidTheta.update(self.odometry.theta, goal_theta, self.time.time())
# self.create.drive_direct(+output_theta, -output_theta)
# self.update_odometry()
# self.create.drive_direct(0, 0)
def turn_left(self):
self.create.drive_direct(self.base_speed, -self.base_speed)
current_theta = self.odometry.theta
while abs(
math.degrees(current_theta) -
math.degrees(self.odometry.theta)) < 90:
self.update_odometry()
self.create.drive_direct(0, 0)
def turn_right(self):
self.create.drive_direct(int(-self.base_speed), int(self.base_speed))
current_theta = self.odometry.theta
while abs(
math.degrees(current_theta) -
math.degrees(self.odometry.theta)) < 90:
self.update_odometry()
self.create.drive_direct(0, 0)
def forward(self, distance):
self.create.drive_direct(int(self.base_speed), int(self.base_speed))
self.sleep(distance / (self.base_speed / 1000))
self.create.drive_direct(0, 0)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
t = self.time.time()
if start + time_in_sec <= t:
break
def update_odometry(self):
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
class StateRBPF:
def __init__(self):
# ----- Set parameters here! ----- #
self.M = 200 # total number of particles パーティクルの数
self.f = 924.1770935 # focus length of camera [px] カメラの焦点距離 [px]
#self.f = 1575.54144 # focus length of camera [px] カメラの焦点距離 [px]
# Particle Filter
self.noise_x_sys = 0.001 # system noise of position (SD) 位置のシステムノイズ（標準偏差）
self.noise_x_sys_coefficient = 0.05 # system noise of position (coefficient) 位置のシステムノイズ（係数）
self.noise_a_sys = 0.1 # system noise of acceleration (SD) 加速度のシステムノイズ（標準偏差）
self.noise_g_sys = 0.01 # system noise of orientation (SD) 角度のシステムノイズ（標準偏差）
self.noise_a_obs = 0.001 # observation noise of acceleration (SD) 加速度の観測ノイズ（標準偏差）
self.noise_g_obs = 0.0001 # observation noise of orientation (SD) 角度の観測ノイズ（標準偏差）
self.noise_camera = 5.0 # observation noise of camera (SD) カメラの観測ノイズ（標準偏差）
self.noise_coplanarity = 0.1 # observation noise of coplanarity (SD) 共面条件の観測ノイズ（標準偏差）
self.init()
def init(self):
self.isFirstTimeIMU = True
self.isFirstTimeCamera = True
self.lock = False
self.t = 0.0
self.t1 = 0.0
self.t_camera = 0.0
self.t1_camera = 0.0
self.accel1 = np.array([0.0, 0.0, 0.0])
self.accel2 = np.array([0.0, 0.0, 0.0])
self.accel3 = np.array([0.0, 0.0, 0.0])
self.gyro = np.array([0.0, 0.0, 0.0])
self.P1 = np.identity(3)
self.initParticleFilter()
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("RBPF")
self.pf.setFocus(self.f)
self.pf.setParameter(self.noise_x_sys, self.noise_a_sys, self.noise_g_sys, self.noise_camera, self.noise_coplanarity, self.noise_x_sys_coefficient) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 1
self.step = 1
def initParticle(self, accel, ori):
X = []
for i in xrange(self.M):
particle = Particle()
particle.initWithIMU(accel, ori)
X.append(particle)
return X
def setObservationModel(self, observation_):
self.pf.setObservationModel(observation_)
"""
This method is called from "sensor.py" when new IMU sensor data are arrived.
time : time (sec)
accel : acceleration in global coordinates
ori : orientaion
"""
def setSensorData(self, time_, accel, ori, gyro_):
# If process is locked by Image Particle Filter, do nothing
if(self.lock):
print("locked")
return
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.gyro = gyro_
if(self.isFirstTimeIMU):
# init particle
self.X = self.initParticle(accel, ori)
else:
# exec particle filter
self.X = self.pf.pf_step_IMU(self.X, self.dt, accel, ori, self.M, self.isFirstTimeCamera)
if(self.isFirstTimeIMU):
self.isFirstTimeIMU = False
# Count
self.count+=1
"""
This method is called from Image class when new camera image data are arrived.
time_ : time (sec)
keypointPairs : list of KeyPointPair class objects
"""
def setImageData(self, time_, keypoints):
# If IMU data has not been arrived yet, do nothing
if(self.isFirstTimeIMU):
return
########################
#print("=================")
#print("step "+str(self.step)+" count "+str(self.count))
###########################
if(keypoints == "nomatch"):
print("nomatch ***********************")
self.reduce_particle_variance(self.X)
self.count += 1
self.step += 1
return
# Lock IMU process
self.lock = True
#start_time = time.clock()
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.t1_camera = self.t_camera
self.t_camera = time_
dt_camera = self.t_camera - self.t1_camera
# covariance matrix of position
P = self.createPositionCovarianceMatrixFromParticle(self.X)
#P *= 0.01
if(self.step > 0 and self.step < 10):
#self.saveXYZasCSV(self.X,"1")
pass
if(self.isFirstTimeCamera):
# exec particle filter
self.X = self.pf.pf_step_camera_firsttime(self.X, self.dt, keypoints, self.step, P, self.M)
else:
# exec particle filter
self.X = self.pf.pf_step_camera(self.X, self.dt, keypoints, self.step, P, self.M, self.X1, self.P1, dt_camera, self.gyro)
if(self.step > 0 and self.step < 10):
#self.saveXYZasCSV(self.X,"2")
pass
# Get prev position and orientation
prevXx, prevXo = self.getPositionAndOrientation()
self.X1 = Particle()
self.X1.initWithPositionAndOrientation(prevXx, prevXo)
self.P1 = P
# Count
self.count += 1
# Step (camera only observation step)
self.step += 1
#end_time = time.clock()
#print "%f" %(end_time-start_time)
# Unlock IMU process
self.lock = False
if(self.isFirstTimeCamera):
self.isFirstTimeCamera = False
def reduce_particle_variance(self, X):
"""
This method is called when No-resampling = True.
Reduce particle variance to avoid divergence of particles.
"""
x = []
# Calc average of position
for X_ in X:
x.append(X_.x)
average = np.mean(x, axis=0)
# Reduce variance of position
for X_ in X:
difference = X_.x - average
X_.x = average + difference * 0.1
return X
"""
print Landmark (X,Y,Z)
"""
def printLandmark(self,X):
print("-----")
landmarks = self.getLandmarkXYZ(X)
for key, value in landmarks.iteritems():
print(str(key)+" "),
print(value)
"""
return Landmark (X,Y,Z)
"""
def getLandmarkXYZ(self,X):
allLandmarks = {}
# calc sum of landmark XYZ
for x in X:
for landmarkId, landmark in x.landmarks.iteritems():
xyz = landmark.getXYZ()
if(allLandmarks.has_key(landmarkId) == False):
allLandmarks[landmarkId] = xyz
else:
allLandmarks[landmarkId] += xyz
# calc average of landamrk XYZ
for key, value in allLandmarks.iteritems():
value /= float(self.M)
return allLandmarks
"""
print (X,Y,Z) of particles
"""
def printXYZ(self,X):
print("-----")
for x in X:
x.printXYZ()
"""
save (X,Y,Z) of particles as CSV file
"""
def saveXYZasCSV(self,X,appendix):
x = []
for X_ in X:
x.append(X_.x)
date = datetime.datetime.now()
#datestr = date.strftime("%Y%m%d_%H%M%S_") + "%04d" % (date.microsecond // 1000)
#np.savetxt('./data/plot3d/'+datestr+'_xyz_'+appendix+'.csv', x, delimiter=',')
datestr = date.strftime("%Y%m%d_%H%M%S_")
np.savetxt('./data/output/particle_'+datestr+str(self.count)+'_'+appendix+'.csv', x, delimiter=',')
"""
create covariance matrix of position from particle set
"""
def createPositionCovarianceMatrixFromParticle(self, X):
x = []
for X_ in X:
if(len(x)==0):
x = X_.x
else:
x = np.vstack((x,X_.x))
P = np.cov(x.T)
return P
"""
return estimated state vector of position and orientation
"""
def getPositionAndOrientation(self):
x = []
o = []
for X_ in self.X:
x.append(X_.x)
o.append(X_.o)
return np.mean(x, axis=0),np.mean(o, axis=0)
"""
This method is called from "Main.py"
return estimated state vector
"""
def getState(self):
x = []
v = []
a = []
o = []
for X_ in self.X:
x.append(X_.x)
v.append(X_.v)
a.append(X_.a)
o.append(X_.o)
#print(np.var(x, axis=0))
return np.mean(x, axis=0),np.mean(v, axis=0),np.mean(a, axis=0),np.mean(o, axis=0)
def data():
print "starting push logic"
p.start("entire_push")
if 'data' not in request.form:
return 'Nothing received'
#Test code:
# request.form = {'data' : json.dumps([
# {'name' : 'sensors',
# 'data' : 'shit'},
# {'name' : 'wifi',
# 'data' : [{'label' : 'blah',
# 'level' : -56,
# 'freqMhz' : 2600,
# 'estimatedDistance' : 10}]}])}
#p.off()
p.start('initial')
data = json.loads(request.form['data'])
wifi_magic = WifiMagic()
wifi_deep_magic = WifiDeeperMagic(cache)
walls = None
p.start('sensors')
sensors_magic = SensorsMagic(walls)
p.pstop('sensors')
p.start('load_particles')
saved_particles = get_db("particles")
p.pstop('load_particles')
pf = ParticleFilter(particles=saved_particles)
p.pstop('initial')
p.start('weights')
for d in data:
if d['name'] == 'sensors':
result = sensors_magic.parse(d['data'])
print 'dheading: %f' % result['dheading']
sensors_magic.update_particles(pf.get_particles(), result)
if d['name'] == 'wifi':
wifidata = d['data']
corr = wifi_deep_magic.get_corrections()
for r in wifidata:
if r['label'] in corr:
oldLvl = r['level']
r['level']+=corr[r['label']]
#print "corrected",r['label'],'from',oldLvl,'to',r['level']
result = wifi_magic.parse(wifidata)
set_db("router_dist", result)
result = wifi_magic.update_particles(pf.get_particles(), result)
p.pstop('weights')
p.start('resample')
pf.resample();
p.pstop('resample')
p.start('save_to_cache')
set_db("particles", pf.get_particles())
p.pstop('save_to_cache')
#x, y = pf.get_position()
#std = pf.get_std()
#print "Particles updated to", x, y, " (var:", std,")"
p.pstop('entire_push')
#return json.dumps([x, y, std])
return 'Thank you!'
transformer = None
"""
:type: tf.Transformer
"""
grid_publisher = None
dynamic_publisher = None
static_publisher = None
dynamics_plot = None
dynamics_image = None
params = ParticleFilterParams(rospy)
particle_filter = ParticleFilter(params)
particle_publisher = None
loader_publisher = None
map = None
class MyTransformer(tf.TransformListener):
def transformPose(self, target_frame, ps, header=None):
if hasattr(ps, "header"):
return super(MyTransformer, self).transformPose(target_frame, ps)
class StateIMUPF: def __init__(self): pass def initWithType(self,PFtype_): self.PFtype = PFtype_ self.init() def init(self): self.isFirstTime = True self.t = 0.0 self.t1 = 0.0 self.initParticleFilter(self.PFtype) def initParticleFilter(self,PFtype): self.pf = ParticleFilter().getParticleFilterClass(PFtype) #PFtype = "IMUPF" or "IMUPF2" self.pf.setParameter(0.01 ,0.001) #パーティクルフィルタのパラメータ（ノイズの分散） variance of noise self.M = 100 # パーティクルの数 num of particles self.X = [] # パーティクルセット set of particles self.loglikelihood = 0.0 self.count = 0 def initParticle(self, accel, ori): X = [] particle = Particle() particle.initWithIMU(accel, ori) for i in range(self.M): X.append(particle) return X """ This method is called from "sensor.py" when new IMU sensor data are arrived. time : time (sec) accel : acceleration in global coordinates ori : orientaion """ def setSensorData(self, time, accel, ori): self.t1 = self.t self.t = time if(self.isFirstTime): # init particle self.X = self.initParticle(accel, ori) else: # exec particle filter self.X = self.pf.pf_step(self.X, self.t - self.t1, accel, ori, self.M) """ The code below is used to get loglikelihood to decide parameters. self.X, likelihood = self.pf.pf_step(self.X, self.t - self.t1, accel, ori, self.M) self.loglikelihood += math.log(likelihood) self.count += 1 if(self.count==300): print(str(self.loglikelihood)) """ if(self.isFirstTime): self.isFirstTime = False """ This method is called from "Main.py" return estimated state vector """ def getState(self): x = [] v = [] a = [] o = [] for X_ in self.X: x.append(X_.x) v.append(X_.v) a.append(X_.a) o.append(X_.o) return np.mean(x, axis=0),np.mean(v, axis=0),np.mean(a, axis=0),np.mean(o, axis=0)
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab8_map.Map("lab8_map.json")
self.robot = MyRobot(self.create, self.time, base_speed=0.1)
self.odometry = odometry.Odometry()
def run(self):
self.create.start()
self.create.safe()
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to visualize the pose of our estimated position
# where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
# self.virtual_create.set_pose((0.5, 0.5, 0.1), 0)
origin = (0.5, 0.5, 0.1, 0) # partical origin
noise = (0.01, 0.05, 0.1) # sd for (distance, theta, sonar)
self.odometry.x = origin[0]
self.odometry.y = origin[1]
self.pf = ParticleFilter(origin, noise, 1000, self.map)
self.virtual_create.set_point_cloud(self.pf.get_particle_list())
# This is an example on how to show particles
# the format is x,y,z,theta,x,y,z,theta,...
# data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0]
# self.virtual_create.set_point_cloud(data)
# This is an example on how to estimate the distance to a wall for the given
# map, assuming the robot is at (0, 0) and has heading math.pi
# print(self.map.closest_distance((0.5,0.5), 0))
# self.positioning()
# This is an example on how to detect that a button was pressed in V-REP
while True:
b = self.virtual_create.get_last_button()
if b == self.virtual_create.Button.MoveForward:
self.forward()
elif b == self.virtual_create.Button.TurnLeft:
self.turn_left()
elif b == self.virtual_create.Button.TurnRight:
self.turn_right()
elif b == self.virtual_create.Button.Sense:
self.sense()
self.time.sleep(0.01)
def forward(self):
print("Forward pressed!")
FORWARD_DISTANCE = 0.5
self.robot.forward(FORWARD_DISTANCE)
self.robot.stop()
self.pf.movement(FORWARD_DISTANCE, 0)
self.virtual_create.set_point_cloud(self.pf.get_particle_list())
self.virtual_create.set_pose((self.pf.xyz), self.pf.theta)
def turn_left(self):
print("Turn Left pressed!")
self.robot.turn_left(1.8)
self.robot.stop()
self.pf.movement(0, math.pi/2)
self.virtual_create.set_point_cloud(self.pf.get_particle_list())
self.virtual_create.set_pose((self.pf.xyz), self.pf.theta)
def turn_right(self):
print("Turn Right pressed!")
self.robot.turn_right(1.8)
self.robot.stop()
self.pf.movement(0, -math.pi/2)
self.virtual_create.set_point_cloud(self.pf.get_particle_list())
self.virtual_create.set_pose((self.pf.xyz), self.pf.theta)
def sense(self):
print("Sense pressed!")
self.pf.sensing(self.sonar.get_distance())
self.virtual_create.set_point_cloud(self.pf.get_particle_list())
self.virtual_create.set_pose((self.pf.xyz), self.pf.theta)
def positioning(self):
for _ in range(21):
self.robot.turn_left(0.3)
self.robot.stop()
self.pf.movement(0, math.pi/12)
self.virtual_create.set_point_cloud(self.pf.get_particle_list())
self.virtual_create.set_pose((self.pf.xyz), self.pf.theta)
self.pf.sensing(self.sonar.get_distance())
self.virtual_create.set_point_cloud(self.pf.get_particle_list())
self.virtual_create.set_pose((self.pf.xyz), self.pf.theta)
class LocalizationNode(object):
'''
Class to hold all ROS related transactions to use split and merge algorithm.
'''
#===========================================================================
def __init__(self, odom_lin_sigma=0.025, odom_ang_sigma=np.deg2rad(2), meas_rng_noise=0.2, meas_ang_noise=np.deg2rad(10), x_init=0, y_init=0, theta_init=0):
'''
Initializes publishers, subscribers and the particle filter.
'''
# Threads
self.lock = threading.RLock()
# Publishers
self.pub_map = rospy.Publisher("map", Marker, queue_size = 2)
self.pub_lines = rospy.Publisher("lines", Marker, queue_size = 2)
self.pub_lines_mean = rospy.Publisher("lines_mean", Marker, queue_size = 2)
self.pub_particles = rospy.Publisher("particles", PoseArray, queue_size = 2)
self.pub_big_particle = rospy.Publisher("mean_particle", PoseStamped, queue_size = 2)
self.pub_odom = rospy.Publisher("mean_particle_odom", Odometry, queue_size = 2)
self.pub_wei = rospy.Publisher("weights", MarkerArray, queue_size = 2)
# Subscribers
self.sub_scan = rospy.Subscriber("lines", Marker, self.laser_callback)
self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
# TF
self.tfBroad = tf.TransformBroadcaster()
# Incremental odometry
self.last_odom = None
self.odom = None
# Flags
self.new_odom = False
self.new_laser = False
self.pub = False
self.time = None
self.lines = None
self.i = 0
# Particle filter
self.part_filter = ParticleFilter(get_dataset3_map(), 500, odom_lin_sigma, odom_ang_sigma, meas_rng_noise, meas_ang_noise, x_init, y_init, theta_init)
#===========================================================================
def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Lock thread
self.lock.acquire()
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = msg.header.stamp,
child = '/base_footprint',
parent = '/world')
# Incremental odometry
if self.last_odom is not None:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
-delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)])
# Flag available
self.cur_odom = msg
self.new_odom = True
# Save initial odometry for increment
else:
self.last_odom = msg
# Unlock thread
self.lock.release()
#===========================================================================
def laser_callback(self, msg):
'''
Reads lines coming from split and merge node.
'''
# Lock thread
self.lock.acquire()
# Save time
self.time = msg.header.stamp
self.lines = None
# Assertion
assert msg.type == Marker.LINE_LIST
if msg.ns == 'scan_line':
# Retrieve lines from split and merge
line = list()
for point in msg.points:
line.append(point.x)
line.append(point.y)
self.lines = np.array(line).reshape((-1, 4))
# Have valid points
if self.lines.shape[0] > 0:
# Flag
self.new_laser = True
else:
self.lines = None
# Unlock thread
self.lock.release()
#===========================================================================
def iterate(self):
'''
Main loop of the filter.
'''
lines = None
# Prediction
if self.new_odom:
# Copy safely
self.lock.acquire()
uk = self.uk.copy()
self.last_odom = self.cur_odom
self.new_odom = False
self.pub = True
self.lock.release()
# Predict filter
self.part_filter.predict(uk)
# Weightimg and resampling
if self.new_laser and self.lines is not None:
# Copy safely
self.lock.acquire()
lines = self.lines.copy()
self.new_laser = False
self.pub = True
self.lock.release()
# Update and resample filter
self.part_filter.weight(lines)
if self.part_filter.moving == True and self.part_filter.n_eff<self.part_filter.num/2.0:
self.part_filter.resample()
# Publish results
if self.pub:
self.publish_results(lines)
self.pub = False
#===========================================================================
def publish_results(self, lines):
'''
Publishes all results from the filter.
'''
if self.time is not None:
time = rospy.Time.now()
# Map of the room
map_lines = get_dataset3_map()
publish_lines(map_lines, self.pub_map, frame='/world', ns='map',
color=(0,1,0))
# Particles and biggest weighted particle
msg, msg_mean, msg_odom, trans, rot, msg_wei = get_particle_msgs(self.part_filter,
time)
self.pub_particles.publish(msg)
self.pub_big_particle.publish(msg_mean)
self.pub_odom.publish(msg_odom)
self.pub_wei.publish(msg_wei)
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = time,
parent = 'world',
child = 'mean_particle')
# Publish scanned lines
if lines is not None:
publish_lines(lines, self.pub_lines_mean, frame='mean_particle',
time=time, ns='scan_lines_mean', color=(0,0,1))
