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
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# Uncomment as completed
self.markers = world_map
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 = 2
self.end_goal = self.goals.shape[0] - 1
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
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
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.pos_goal = pos_goal
# YOUR CODE AFTER THIS
#-------------------------------------------#
self.time = rospy.get_time()
self.controlOut = (0.0, 0.0, False)
self.count_noMeasurement = 0
#-------------------------------------------#
self.markers = world_map
self.idx_target_marker = 0
# Calculate the optimal path
# From pos_init to pos_goal
self.path_2D = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.idx_path = 0
self.size_path = self.path_2D.shape[0]
print "path.shape[0]", self.size_path
# Generate the 3D path (include "theta")
self.path = np.zeros((self.size_path, 3))
theta = 0.0
for idx in range(self.size_path - 1):
delta = self.path_2D[(idx + 1), :] - self.path_2D[idx, :]
theta = atan2(delta[1], delta[0])
if theta > np.pi:
theta -= np.pi * 2
elif theta < -np.pi:
theta += np.pi * 2
self.path[idx, :] = np.concatenate(
(self.path_2D[idx, :], np.array([theta])), axis=1)
self.path[self.size_path - 1, 0:2] = self.path_2D[self.size_path - 1,
0:2]
self.path[self.size_path - 1, 2] = pos_goal[2] # theta
#
self.path[0, 2] = pos_init[2] # theta
print "3D path:"
print self.path
# Uncomment as completed
# Kalman filter
self.kalman_filter = KalmanFilter(world_map)
self.kalman_filter.mu_est = pos_init # 3*pos_init # For test
# Differential drive
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.wayPointFollowing = wayPointFollowing(max_speed, max_omega)
#
self.task_done = False
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
# speed control variables
self.v = 0.1 # allows persistent cmds through detection misses
self.omega = -0.1 # allows persistent cmds through detection misses
self.last_detect_time = rospy.get_time() #TODO on bot only
self.missed_vision_debounce = 1
self.start_time = 0
# generate the path assuming we know our start location, goal, and environment
self.path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.path_idx = 0
self.mission_complete = False
self.carrot_distance = 0.22
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
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 __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 main(args):
rospy.init_node('robot_control', anonymous=True)
param_path = rospy.get_param("~param_path")
f = open(param_path, 'r')
params_raw = f.read()
f.close()
params = yaml.load(params_raw)
occupancy_map = np.array(params['occupancy_map'])
world_map = np.array(params['world_map'])
pos_init = np.array(params['pos_init'])
pos_goal = np.array(params['pos_goal'])
max_vel = params['max_vel']
max_omega = params['max_omega']
t_cam_to_body = np.array(params['t_cam_to_body'])
x_spacing = params['x_spacing']
y_spacing = params['y_spacing']
# Intialize the RobotControl object
robotControl = RobotControl(world_map, occupancy_map, pos_init, pos_goal,
max_vel, max_omega, x_spacing, y_spacing,
t_cam_to_body)
# Create path
path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal)
print(path)
# Call process_measurements at 60Hz
r = rospy.Rate(60)
for waypoint in path:
print(waypoint)
if rospy.is_shutdown():
break
while True and not rospy.is_shutdown():
done = robotControl.process_measurements(waypoint)
if done == True:
break
r.sleep()
# Done, stop robot
robotControl.ros_interface.command_velocity(0, 0)