class RobotControl(object):
"""Class used to interface with the rover. Gets sensor measurements
through ROS subscribers, and transforms them into the 2D plane,
and publishes velocity commands.
"""
def __init__(self, markers, occupancy_map, pos_init, pos_goal, max_speed,
min_speed, max_omega, x_spacing, y_spacing, t_cam_to_body,
mode):
"""
Initialize the class
"""
# plan a path around obstacles using dijkstra's algorithm
print('Planning path...')
path = findShortestPath(occupancy_map,
x_spacing,
y_spacing,
pos_init[0:2],
pos_goal[0:2],
dilate=2)
print('Done!')
self.path_manager = PathManager(path)
self.kalman_filter = KalmanFilter(markers, pos_init)
self.diff_drive_controller = DiffDriveController(
max_speed, min_speed, max_omega)
if 'HARDWARE' in mode:
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
elif 'SIMULATE' in mode:
self.robot_sim = RobotSim(markers, occupancy_map, pos_init,
pos_goal, max_speed, max_omega,
x_spacing, y_spacing,
self.path_manager.path, mode)
self.user_control = UserControl()
self.vel = 0 # save velocity to use for kalman filter
self.goal = self.path_manager.getNextWaypoint() # get first waypoint
# for logging postion data to csv file
self.stateSaved = []
self.tagsSaved = []
self.waypoints = []
def process_measurements(self):
"""Main loop of the robot - where all measurements, control, and
estimation are done.
"""
v, omega, wayptReached = self.user_control.compute_vel()
self.robot_sim.command_velocity(v, omega)
return
#meas = None # for testing purposes
#imu_meas = None # for testing purpose
#pdb.set_trace()
if (imu_meas is None) and (meas is None):
pass
else:
state = self.kalman_filter.step_filter(self.vel, imu_meas, meas)
if 'SIMULATE' in mode:
self.robot_sim.set_est_state(state)
#print("X = {} cm, Y = {} cm, Theta = {} deg".format(100*state[0],100*state[1],state[2]*180/np.pi))
# save the estimated state and tag statuses for offline animation
self.stateSaved.append(state)
self.waypoints.append(self.path_manager.getActiveWaypointsPos())
if meas is None:
self.tagsSaved.append(None)
else:
meas = np.array(meas)
tagIDs = [int(i) for i in meas[:, 3]]
self.tagsSaved.append(tagIDs)
v, omega, wayptReached = self.diff_drive_controller.compute_vel(
state, self.goal.pos)
self.vel = v
if wayptReached:
self.goal = self.path_manager.getNextWaypoint()
self.diff_drive_controller.done = False # reset diff controller status
if self.goal is None:
print('Goal has been reached!')
np.savez('savedState.npz',
stateSaved=self.stateSaved,
tagsSaved=self.tagsSaved,
waypoints=self.waypoints)
print('Position data saved!')
if 'HARDWARE' in mode:
self.ros_interface.command_velocity(0, 0)
elif 'SIMULATE' in mode:
self.robot_sim.command_velocity(0, 0)
self.robot_sim.done = True
return
else:
if 'HARDWARE' in mode:
self.ros_interface.command_velocity(self.vel, omega)
elif 'SIMULATE' in mode:
self.robot_sim.command_velocity(self.vel, omega)
return
def myhook():
print "shutdown time!"
