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)