def main():
global P
global CMD_VEL
# Uncomment for debugging if nesssary, recomment before turning in.
# rp.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)
rp.Subscriber('/robot/base_scan', LaserScan, got_scan) # movement/sensor particle update
rp.Subscriber('/robot/cmd_vel', Twist, got_vel)
#actual robot pose only used to show it on map not for partcile filter
rp.Subscriber('/stage/base_pose_ground_truth', Odometry, actual_pose) #actual robot pose
#removed due to wander node
#CMD_VEL = rp.Publisher('/robot/cmd_vel', Twist, queue_size=1)
# create initial particles
for p in xrange(PARTICLE_COUNT):
r = ROBOT()
r.set_pose(5, 5.6, 0) # map space
P.append(r)
mcl_tools.mcl_init('particle_2d')
mcl_tools.mcl_run_viz()
def particle_filter(ps, control, scan):
# FIXME: Should really particle filter.
return [mcl_tools.random_particle() for ii in range(PAR_COUNT)]
def got_scan(msg):
global parset
global cmd_vel
control = (0.0, 0.0)
parset = particle_filter(parset, control, msg)
mcl_tools.show_particles(parset)
cmd = Twist()
(cmd.linear.x, cmd.angular.z) = control
cmd_vel.publish(cmd)
if __name__ == '__main__':
# Uncomment for debugging if nesssary, recomment before turning in.
#rospy.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)
rospy.Subscriber('/robot/base_scan', LaserScan, got_scan)
cmd_vel = rospy.Publisher('/robot/cmd_vel', Twist, queue_size=1)
mcl_tools.mcl_init('with_weights')
mcl_tools.mcl_run_viz()
centroid = motion_update(centroid)
rospy.sleep(0.1)
if __name__ == '__main__':
# Uncomment for debugging if nesssary, recomment before turning in.
#rospy.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)
try:
import psyco
psyco.full()
except IOError:
pass
cmd_vel = rospy.Publisher('/robot/cmd_vel', Twist)
rospy.Subscriber('/robot/base_scan',
LaserScan,
callback=got_scan,
queue_size=10,
tcp_nodelay=True)
rospy.Subscriber('/robot/base_scan',
LaserScan,
callback=controller,
queue_size=10,
tcp_nodelay=True)
clustering_thread = threading.Thread(target=clustering)
clustering_thread.start()
mcl_tools.mcl_init('sample_hw7')
mcl_tools.mcl_run_viz()
def got_scan(msg):
global parset, odometry
if odometry is not None:
# print odometry
linear_speed = odometry.twist.twist.linear.x
angular_speed = odometry.twist.twist.angular.z
control = (linear_speed, angular_speed)
# print control
parset = particle_filter(parset, control, msg) # run particle filter
mcl_tools.show_particles(parset) # render particle cloud
else:
print "odometry is none"
def got_odom(msg):
global odometry
odometry = msg
def got_map(msg):
print msg
if __name__ == '__main__':
rp.Subscriber('/scan', LaserScan, got_scan, queue_size=1)
rp.Subscriber('/odom', Odometry, got_odom, queue_size=1)
# rp.Subscriber('/map', )
mcl_tools.mcl_init('localizer_node')
mcl_tools.mcl_run_viz()
def particle_filter(ps, control, scan):
# FIXME: Should really particle filter.
return [mcl_tools.random_particle() for ii in range(PAR_COUNT)]
def got_scan(msg):
global parset
global cmd_vel
control = (0.0, 0.0)
parset = particle_filter(parset, control, msg)
mcl_tools.show_particles(parset)
cmd = Twist()
(cmd.linear.x, cmd.angular.z) = control
cmd_vel.publish(cmd)
if __name__ == '__main__':
# Uncomment for debugging if nesssary, recomment before turning in.
#rospy.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)
rospy.Subscriber('/robot/base_scan', LaserScan, got_scan)
cmd_vel = rospy.Publisher('/robot/cmd_vel', Twist, queue_size=1)
mcl_tools.mcl_init('no_weights')
mcl_tools.mcl_run_viz()
Wait = False
rospy.sleep(0.1)
for i in xrange(20):
centroid = motion_update(centroid)
rospy.sleep(0.1)
if __name__ == '__main__':
# Uncomment for debugging if nesssary, recomment before turning in.
#rospy.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)
try:
import psyco
psyco.full()
except IOError:
pass
cmd_vel = rospy.Publisher('/robot/cmd_vel', Twist)
rospy.Subscriber('/robot/base_scan', LaserScan, callback=got_scan,
queue_size = 10, tcp_nodelay = True)
rospy.Subscriber('/robot/base_scan', LaserScan, callback=controller,
queue_size = 10, tcp_nodelay = True)
clustering_thread = threading.Thread(target=clustering)
clustering_thread.start()
mcl_tools.mcl_init('sample_hw7')
mcl_tools.mcl_run_viz()