def got_scan(msg):
global Wait
global Found_Goal
if Wait or Found_Goal:
return
global parset
global control
global centroid
laserscan = msg.ranges
'''
enum_ls = [(s, i) for i, s in enumerate(laserscan)]
max_ls_index = max(enum_ls)[1]
min_ls_val, min_ls_index = min(enum_ls)
print max_ls_index
control = (min_ls_val*0.9 - 1, (max_ls_index-(min_ls_index*0.55))*.85)
'''
# Calculate command to robot.
# if front sensor reading is greater than 3, take the difference between
# left and right diagnal sensors.
# Otherwise, slow down and go in whichever direction has more room.
parset = particle_filter(parset, control, laserscan)
mcl_tools.show_particles(parset)
mcl_tools.show_best_pose(centroid)
return
def got_scan(msg):
global Wait
global Found_Goal
if Wait or Found_Goal:
return
global parset
global control
global centroid
laserscan = msg.ranges
'''
enum_ls = [(s, i) for i, s in enumerate(laserscan)]
max_ls_index = max(enum_ls)[1]
min_ls_val, min_ls_index = min(enum_ls)
print max_ls_index
control = (min_ls_val*0.9 - 1, (max_ls_index-(min_ls_index*0.55))*.85)
'''
# Calculate command to robot.
# if front sensor reading is greater than 3, take the difference between
# left and right diagnal sensors.
# Otherwise, slow down and go in whichever direction has more room.
parset = particle_filter(parset, control, laserscan)
mcl_tools.show_particles(parset)
mcl_tools.show_best_pose(centroid)
return
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)
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_scan(msg):
"""
This function updates on each message received from laser scan
Command robot in simulation space and pass information to particle filter
:param msg: contains message of laser scan data
"""
global P, CMD_VEL, CONTROL, PRIOR_TIME, BEAMS
# control simulated robot movement
if BEAMS is None: # statically use it after initialization (message shouldn't change length while it's running)
BEAMS = np.multiply(range(len(msg.ranges)), msg.angle_increment) + msg.angle_min
#print rp.get_rostime()
P = particle_filter(P, CONTROL, msg)
#print P
mcl_tools.show_particles(P)