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, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.markers = world_map
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
self.vel = np.array([0, 0])
self.imu_meas = np.array([])
self.meas = []
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
self.goals = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal)
self.total_goals = self.goals.shape[0]
self.cur_goal = 1
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
# TODO for student: Comment this when running on the robot
"""
measurements - a N by 5 list of visible tags or None. The tags are in
the form in the form (x,y,theta,id,time) with x,y being the 2D
position of the marker relative to the robot, theta being the
relative orientation of the marker with respect to the robot, id
being the identifier from the map, and time being the current time
stamp. If no tags are seen, the function returns None.
"""
# meas: measurements coming from tags
meas = self.robot_sim.get_measurements()
self.meas = meas;
# imu_meas: measurment comig from the imu
imu_meas = self.robot_sim.get_imu()
self.imu_meas = imu_meas
pose = self.kalman_filter.step_filter(self.vel, self.imu_meas, np.asarray(self.meas))
self.robot_sim.set_est_state(pose)
# goal = self.goals[self.cur_goal]
# vel = self.diff_drive_controller.compute_vel(pose, goal)
# self.vel = vel[0:2]
# self.robot_sim.command_velocity(vel[0], vel[1])
# close_enough = vel[2]
# if close_enough:
# print 'goal reached'
# if self.cur_goal < (self.total_goals - 1):
# self.cur_goal = self.cur_goal + 1
# vel = (0, 0)
# self.vel = vel
# self.robot_sim.command_velocity(vel[0], vel[1])
# else:
# vel = (0, 0)
# self.vel = vel
# self.robot_sim.command_velocity(vel[0], vel[1])
# Code to follow AprilTags
if(meas != None and meas):
cur_meas = meas[0]
tag_robot_pose = cur_meas[0:3]
tag_world_pose = self.tag_pos(cur_meas[3])
state = self.robot_pos(tag_world_pose, tag_robot_pose)
goal = tag_world_pose
vel = self.diff_drive_controller.compute_vel(pose, goal)
self.vel = vel[0:2];
if(not vel[2]):
self.robot_sim.command_velocity(vel[0], vel[1])
else:
vel = (0.1, 0.1)
self.vel = vel
self.robot_sim.command_velocity(vel[0], vel[1])
else:
vel = (0.1, 0.1)
self.vel = vel
self.robot_sim.command_velocity(vel[0], vel[1])
# goal = [-0.5, 2.5]
# goal = self.goals[self.cur_goal]
# vel = self.diff_drive_controller.compute_vel(pose, goal)
# self.vel = vel[0:2];
# if(not vel[2]):
# self.robot_sim.command_velocity(vel[0], vel[1])
# else:
# vel = (0, 0)
# if self.cur_goal < (self.total_goals - 1):
# self.cur_goal = self.cur_goal + 1
# self.vel = vel
# TODO for student: Use this when transferring code to robot
# meas = self.ros_interface.get_measurements()
# imu_meas = self.ros_interface.get_imu()
return
def tag_pos(self, marker_id):
for i in range(len(self.markers)):
marker_i = np.copy(self.markers[i])
if marker_i[3] == marker_id:
return marker_i[0:3]
return None
def robot_pos(self, w_pos, r_pos):
H_W = np.array([[math.cos(w_pos[2]), -math.sin(w_pos[2]), w_pos[0]],
[math.sin(w_pos[2]), math.cos(w_pos[2]), w_pos[1]],
[0, 0, 1]])
H_R = np.array([[math.cos(r_pos[2]), -math.sin(r_pos[2]), r_pos[0]],
[math.sin(r_pos[2]), math.cos(r_pos[2]), r_pos[1]],
[0, 0, 1]])
w_r = H_W.dot(inv(H_R))
robot_pose = np.array([[w_r[0,2]], [w_r[1,2]], [math.atan2(w_r[1,0], w_r[0, 0])]])
return robot_pose
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, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
#self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
# TODO for student: Comment this when running on the robot
meas = self.robot_sim.get_measurements()
imu_meas = self.robot_sim.get_imu()
self.robot_sim.command_velocity(0.3, 0)
# TODO for student: Use this when transferring code to robot
# meas = self.ros_interface.get_measurements()
# imu_meas = self.ros_interface.get_imu()
return
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, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.markers = world_map
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
# TODO for student: Comment this when running on the robot
"""
measurements - a N by 5 list of visible tags or None. The tags are in
the form in the form (x,y,theta,id,time) with x,y being the 2D
position of the marker relative to the robot, theta being the
relative orientation of the marker with respect to the robot, id
being the identifier from the map, and time being the current time
stamp. If no tags are seen, the function returns None.
"""
meas = self.robot_sim.get_measurements()
imu_meas = self.robot_sim.get_imu()
if (meas != None and meas):
cur_meas = meas[0]
goal = np.array(self.tag_pos(cur_meas[3]))
state = np.array([
goal[0] - cur_meas[0], goal[1] - cur_meas[1],
np.arctan2(cur_meas[1], cur_meas[0]) - cur_meas[2]
])
vel = self.diff_drive_controller.compute_vel(state, goal)
if (not vel[2]):
self.robot_sim.command_velocity(vel[0], vel[1])
else:
self.robot_sim.command_velocity(0, 0)
else:
self.robot_sim.command_velocity(0.0, 0.1)
# TODO for student: Use this when transferring code to robot
# meas = self.ros_interface.get_measurements()
# imu_meas = self.ros_interface.get_imu()
return
def tag_pos(self, marker_id):
for i in range(len(self.markers)):
marker_i = np.copy(self.markers[i])
if marker_i[3] == marker_id:
return marker_i[0:2]
return None
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, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.markers = world_map
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
# TODO for student: Comment this when running on the robot
"""
measurements - a N by 5 list of visible tags or None. The tags are in
the form in the form (x,y,theta,id,time) with x,y being the 2D
position of the marker relative to the robot, theta being the
relative orientation of the marker with respect to the robot, id
being the identifier from the map, and time being the current time
stamp. If no tags are seen, the function returns None.
"""
meas = self.robot_sim.get_measurements()
imu_meas = self.robot_sim.get_imu()
if(meas != None and meas):
cur_meas = meas[0]
goal = np.array(self.tag_pos(cur_meas[3]))
state = np.array([goal[0] - cur_meas[0], goal[1] - cur_meas[1], np.arctan2(cur_meas[1], cur_meas[0]) - cur_meas[2]])
vel = self.diff_drive_controller.compute_vel(state, goal)
if(not vel[2]):
self.robot_sim.command_velocity(vel[0], vel[1])
else:
self.robot_sim.command_velocity(0, 0)
else:
self.robot_sim.command_velocity(0.0, 0.1)
# TODO for student: Use this when transferring code to robot
# meas = self.ros_interface.get_measurements()
# imu_meas = self.ros_interface.get_imu()
return
def tag_pos(self, marker_id):
for i in range(len(self.markers)):
marker_i = np.copy(self.markers[i])
if marker_i[3] == marker_id:
return marker_i[0:2]
return None
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, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
plan = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal)
self.state_tol = 0.1
self.path = plan.tolist()
print "Path: ", self.path, type(self.path)
self.path.reverse()
self.path.pop()
self.state = pos_init
self.goal = self.path.pop()
self.x_offset = x_spacing
self.vw = (0, 0, False)
# self.goal[0] += self.x_offset/2
# self.goal[1] += y_spacing
print "INIT GOAL: ", self.goal
# def dijkstras(occupancy_map, x_spacing, y_spacing, start, goal):
def process_measurements(self):
"""
Main loop of the robot - where all measurements, control, and estimation
are done. This function is called at 60Hz
"""
meas = self.robot_sim.get_measurements()
imu_meas = self.robot_sim.get_imu()
self.vw = self.diff_drive_controller.compute_vel(self.state, self.goal)
print "VW: ", self.vw
print "Running Controller."
if self.vw[2] == False:
self.robot_sim.command_velocity(self.vw[0], self.vw[1])
else:
self.robot_sim.command_velocity(0, 0)
est_x = self.kalman_filter.step_filter(self.vw, imu_meas, meas)
print "EST X: ", est_x, est_x[2][0]
if est_x[2][0] > 2.617991667:
est_x[2][0] = 2.617991667
if est_x[2][0] < 0.523598333:
est_x[2][0] = 0.523598333
self.state = est_x
print "Get GT Pose: ", self.robot_sim.get_gt_pose()
print "EKF Pose: ", est_x
self.robot_sim.get_gt_pose()
self.robot_sim.set_est_state(est_x)
if imu_meas != None:
self.kalman_filter.prediction(self.vw, imu_meas)
if meas != None and meas != []:
print("Measurements: ", meas)
if imu_meas != None:
# self.kalman_filter.prediction(self.vw, imu_meas)
self.kalman_filter.update(meas)
pos_x_check = ((self.goal[0] + self.state_tol) > est_x.item(0)) and \
((self.goal[0] - self.state_tol) < est_x.item(0))
pos_y_check = ((self.goal[1] + self.state_tol) > est_x.item(1)) and \
((self.goal[1] - self.state_tol) < est_x.item(1))
if pos_x_check and pos_y_check:
if self.path != []:
self.goal = self.path.pop()
# self.goal[0] += self.x_offset/2
# self.goal[1] += y_spacing
else:
self.goal = est_x
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, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.markers = world_map
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
self.vel = np.array([0, 0])
self.imu_meas = np.array([])
self.meas = []
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
self.goals = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.total_goals = self.goals.shape[0]
self.cur_goal = 1
self.kalman_filter = KalmanFilter(world_map)
self.kalman_SLAM = KalmanFilterSLAM()
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
# TODO for student: Comment this when running on the robot
"""
measurements - a N by 5 list of visible tags or None. The tags are in
the form in the form (x,y,theta,id,time) with x,y being the 2D
position of the marker relative to the robot, theta being the
relative orientation of the marker with respect to the robot, id
being the identifier from the map, and time being the current time
stamp. If no tags are seen, the function returns None.
"""
# meas: measurements coming from tags
meas = self.robot_sim.get_measurements()
self.meas = meas
# imu_meas: measurment comig from the imu
imu_meas = self.robot_sim.get_imu()
self.imu_meas = imu_meas
pose = self.kalman_filter.step_filter(self.vel, self.imu_meas,
np.asarray(self.meas))
pose_map = self.kalman_SLAM.step_filter(self.vel, self.imu_meas,
np.asarray(self.meas))
print 'pose'
print pose
print 'pose & map'
print pose_map
self.robot_sim.set_est_state(pose)
# goal = self.goals[self.cur_goal]
# vel = self.diff_drive_controller.compute_vel(pose, goal)
# self.vel = vel[0:2]
# self.robot_sim.command_velocity(vel[0], vel[1])
# close_enough = vel[2]
# if close_enough:
# print 'goal reached'
# if self.cur_goal < (self.total_goals - 1):
# self.cur_goal = self.cur_goal + 1
# vel = (0, 0)
# self.vel = vel
# self.robot_sim.command_velocity(vel[0], vel[1])
# else:
# vel = (0, 0)
# self.vel = vel
# self.robot_sim.command_velocity(vel[0], vel[1])
# Code to follow AprilTags
# if(meas != None and meas):
# cur_meas = meas[0]
# tag_robot_pose = cur_meas[0:3]
# tag_world_pose = self.tag_pos(cur_meas[3])
# state = self.robot_pos(tag_world_pose, tag_robot_pose)
# goal = tag_world_pose
# vel = self.diff_drive_controller.compute_vel(pose, goal)
# self.vel = vel[0:2];
# if(not vel[2]):
# self.robot_sim.command_velocity(vel[0], vel[1])
# else:
# vel = (0, 0)
# self.vel = vel
# self.robot_sim.command_velocity(vel[0], vel[1])
# goal = [-0.5, 2.5]
goal = self.goals[self.cur_goal]
vel = self.diff_drive_controller.compute_vel(pose, goal)
self.vel = vel[0:2]
if (not vel[2]):
self.robot_sim.command_velocity(vel[0], vel[1])
else:
vel = (0, 0)
if self.cur_goal < (self.total_goals - 1):
self.cur_goal = self.cur_goal + 1
self.vel = vel
self.robot_sim.command_velocity(vel[0], vel[1])
# TODO for student: Use this when transferring code to robot
# meas = self.ros_interface.get_measurements()
# imu_meas = self.ros_interface.get_imu()
return
def tag_pos(self, marker_id):
for i in range(len(self.markers)):
marker_i = np.copy(self.markers[i])
if marker_i[3] == marker_id:
return marker_i[0:3]
return None
def robot_pos(self, w_pos, r_pos):
H_W = np.array([[math.cos(w_pos[2]), -math.sin(w_pos[2]), w_pos[0]],
[math.sin(w_pos[2]),
math.cos(w_pos[2]), w_pos[1]], [0, 0, 1]])
H_R = np.array([[math.cos(r_pos[2]), -math.sin(r_pos[2]), r_pos[0]],
[math.sin(r_pos[2]),
math.cos(r_pos[2]), r_pos[1]], [0, 0, 1]])
w_r = H_W.dot(inv(H_R))
robot_pose = np.array([[w_r[0, 2]], [w_r[1, 2]],
[math.atan2(w_r[1, 0], w_r[0, 0])]])
return robot_pose
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, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
self.pos_goal = pos_goal
self.world_map = world_map
self.vel = np.array([0., 0.])
self.imu_meas = np.array([])
self.meas = []
self.max_speed = max_speed
self.max_omega = max_omega
self.goals = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
# print(self.goals)
self.total_goals = self.goals.shape[0]
self.cur_goal = 2
self.end_goal = self.goals.shape[0] - 1
self.est_pose = None
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def process_measurements(self):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
# TODO for student: Comment this when running on the robot
meas = self.robot_sim.get_measurements()
imu_meas = self.robot_sim.get_imu()
# TODO for student: Use this when transferring code to robot
# meas = self.ros_interface.get_measurements()
# imu_meas = self.ros_interface.get_imu()
self.est_pose = self.kalman_filter.step_filter(self.vel, imu_meas,
np.asarray(meas))
# print(self.est_pose)
# set goal
# print(self.cur_goal)
self.pos_goal = self.goals[self.cur_goal]
# get command
v, w, done = self.diff_drive_controller.compute_vel(
self.est_pose, self.pos_goal)
# while not at goal (waypoint), command velocity
if done:
v, w = (0, 0)
if self.cur_goal < self.end_goal:
self.cur_goal = self.cur_goal + 1
done = False
#self.ros_interface.command_velocity(v,w)
self.robot_sim.command_velocity(v, w)
self.robot_sim.done = done
self.vel = np.array([v, w])
# print(done)
return
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, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
self.switch_to_state("go_to_goal")
# Uncomment as completed
#self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def switch_to_state(self, state):
self.current_state = state
def process_measurements(self, pos_goal):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
# TODO for student: Comment this when running on the robot
# (x,y,theta,id,time) with x,y being the 2D
# position of the marker relative to the robot, theta being the
# relative orientation of the marker with respect to the robot, id
# being the identifier from the map, and time being the current time
# stamp. If no tags are seen, the function returns None.
meas = self.robot_sim.get_measurements()
# imu_meas = self.robot_sim.get_imu()
if True:
pose_robot_in_world = self.robot_sim.get_gt_pose()
# print "pose_robot = ", pose_robot_in_world
v, omega, at_goal = self.diff_drive_controller.compute_vel(
pose_robot_in_world, pos_goal)
# print "v = ", v
# print "omega = ", omega
# print "at_goal = ", at_goal
elif meas is not None and meas != []:
#print meas
#print type(meas)
tag_id = int(meas[0][3]) # get the id of the tag seen
# print "tag_id = ", tag_id
pose_tag_in_robot = meas[0][0:3] # get pose of tag in robot frame
H_RT = util.get_transform_matrix(
pose_tag_in_robot
) # get transform matrix of robot to tag frame
# print "markers = ", self.robot_sim.markers
# print "type(markers) = ", type(self.robot_sim.markers)
# print "markers[tag_id] = ", self.robot_sim.markers[tag_id]
pose_tag_in_world = self.robot_sim.markers[
tag_id, 0:3] # get pose of tag in world frame
# print "pose_tag_in_world = ", pose_tag_in_world
H_WT = util.get_transform_matrix(
pose_tag_in_world
) # get transform matrix of world to tag frame
H_TR = np.linalg.inv(
H_RT) # get transform matrix of tag to robot frame
H_WR = np.dot(H_WT,
H_TR) # get transform matrix of world to robot frame
pose_robot_in_world = util.get_pose_from_transform(H_WR)
# print "pose_robot_in_world = ", pose_robot_in_world
# print "ground truth pose = ", self.robot_sim.get_gt_pose()
v, omega, at_goal = self.diff_drive_controller.compute_vel(
pose_robot_in_world, self.pos_goal)
#print done
self.robot_sim.command_velocity(v, omega)
# TODO for student: Use this when transferring code to robot
# meas = self.ros_interface.get_measurements()
# imu_meas = self.ros_interface.get_imu()
return at_goal
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, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body, sample_time):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
sample_time - the sample time in seconds between each measurement (added by me)
"""
# TODO for student: Comment this when running on the robot
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map, sample_time)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.v_last = 0.0
self.omega_last = 0.0
def process_measurements(self, goal):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
# TODO for student: Comment this when running on the robot
meas = np.array(self.robot_sim.get_measurements()) # measured pose of tag in robot frame (x,y,theta,id,time)
imu_meas = self.robot_sim.get_imu() # 5 by 1 numpy vector (acc_x, acc_y, acc_z, omega, time)
# Do KalmanFilter step
# Note: The imu_meas could be None if it is sampled at a lower rate than the integration time step.
# For the simulation, assume that imu_meas will always return a valid value because we control it in RobotSim.py.
# For running on the robot, you should include protection for potentially bad measurements.
pose_est = self.kalman_filter.step_filter(self.v_last, imu_meas, meas)
at_goal = False
v, omega, at_goal = self.diff_drive_controller.compute_vel(pose_est, goal)
self.robot_sim.command_velocity(v, omega)
self.v_last = v
self.omega_last = omega
#print("estimated state = ", est_state)
#print("true state = ", self.robot_sim.get_gt_pose())
# Draw ghost robot
est_state = np.array([[pose_est[0]],[pose_est[1]],[pose_est[2]]])
self.robot_sim.set_est_state(est_state)
# TODO for student: Use this when transferring code to robot
# meas = self.ros_interface.get_measurements()
# imu_meas = self.ros_interface.get_imu()
return at_goal
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, world_map,occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.v_last = 0.0
self.omega_last = 0.0
def process_measurements(self):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and esimtaiton
are done. This function is called at 60Hz
"""
# TODO for student: Comment this when running on the robot
meas = np.array(self.robot_sim.get_measurements()) # tag wrt robot in robot frame (x,y,theta,id,time)
imu_meas = self.robot_sim.get_imu() # (5,1) np array (xacc,yacc,zacc,omega,time)
done = False
goal = np.array([1., 1.2])
# z_t is the Nx4 numpy array (x,y,theta,id) of tag wrt robot
if meas is not None and meas != []:
#print(meas)
#print(imu_meas)
z_t = meas[:,:-1] # (3, 4) np array (x, y, theta, id)
else:
z_t = meas
#print("v_last = ", self.v_last)
#print("imu_meas = ", imu_meas)
#print("z_t = ", z_t)
state = self.kalman_filter.step_filter(self.v_last, self.omega_last, imu_meas, z_t)
#print("estimated state = ", state)
#print("true state = ", self.robot_sim.get_gt_pose())
# Draw ghost robot
est_pose = np.array([[state[0]],[state[1]],[state[2]]])
self.robot_sim.set_est_state(est_pose)
if done is False:
v, omega, done = self.diff_drive_controller.compute_vel(state, goal)
self.robot_sim.command_velocity(v, omega)
self.v_last = v
self.omega_last = omega
#if meas is not None and meas != []:
#print meas
#state = np.array(meas[0][0:3])
#goal = np.array([0, 0])
#v, omega, done = self.diff_drive_controller.compute_vel(state, goal)
#self.robot_sim.command_velocity(v, omega)
# TODO for student: Use this when transferring code to robot
# meas = self.ros_interface.get_measurements()
# imu_meas = self.ros_interface.get_imu()
return done