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)
def __init__(self, pos_init, pos_goal, max_speed, max_omega): """ Initialize the class """ # Handles all the ROS related items self.ros_interface = ROSInterface() time.sleep(0.2) self.max_speed = max_speed self.max_omega = max_omega self.prev_lin_vel = 0.0 self.prev_ang_vel = 0.0 self.pos_init = pos_init self.pos_goal = pos_goal #self.joy = xbox.Joystick() #Initialize joystick self.state = [pos_init[0], pos_init[1], 0.0] #Kalman initialization self.kalman_filter = KalmanFilter(dim_x=3, dim_z=3) self.kalman_filter.F = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # State Transition Matrix self.kalman_filter.H = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # Measurement function self.kalman_filter.P = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]]) # covariance matrix self.kalman_filter.R = np.array([[0.035, 0, 0], [0, 0.035, 0], [0, 0, 0.001]]) # state uncertainty self.kalman_filter.Q = np.array([[0.005, 0, 0], [0, 0.005, 0], [0, 0, 0.008]]) # process uncertainty self.kalman_filter.x = np.array([self.state]).T
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 """ # 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 This function is called at 60Hz """ meas = self.ros_interface.get_measurements() imu_meas = self.ros_interface.get_imu() self.ros_interface.command_velocity(0, 0) return
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)
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 """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) self.kalman_filter = KalmanFilter(world_map) print("INITSTATE", self.kalman_filter.state) self.diff_drive_controller = DiffDriveController(max_speed, max_omega) self.vel = 0 self.omega = 0 self.curInd = 0 self.path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal) print(self.path) self.curGoal = self.path[0] self.done = False def process_measurements(self): """ This function is called at 60Hz """ meas = self.ros_interface.get_measurements() print("Mesurements", meas) imu_meas = self.ros_interface.get_imu() print(imu_meas) updatedPosition = self.kalman_filter.step_filter( self.vel, self.omega, imu_meas, meas) print(np.linalg.norm(self.curGoal - updatedPosition[0:1])) if ((np.abs(self.curGoal[0] - updatedPosition[0]) > 0.1) or (np.abs(self.curGoal[1] - updatedPosition[1]) > 0.1)): (v, omega, done) = self.diff_drive_controller.compute_vel( updatedPosition, self.curGoal) self.vel = v self.omega = omega print("commanded vel:", v, omega) self.ros_interface.command_velocity(v, omega) else: print("updating") self.curInd = self.curInd + 1 if self.curInd < len(self.path): self.curGoal = self.path[self.curInd] else: self.done = True updatedPosition.shape = (3, 1) return
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.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) # YOUR CODE AFTER THIS self.markers = world_map self.vel = np.array([0., 0.]) self.imu_meas = np.array([]) self.meas = []
def __init__(self, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body, min_lin_x, max_lin_x, rotate_fast_value, linear_k, k, min_linear_vel): """ Initialize the class """ self.min_lin_x = min_lin_x self.max_lin_x = max_lin_x self.rotate_fast_value = rotate_fast_value self.linear_k = linear_k self.k = k self.min_linear_vel = min_linear_vel # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body)
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)
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 """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) # YOUR CODE AFTER THIS self.markers = 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) # self.total_goals = self.goals.shape[0] # self.cur_goal = 2 # self.end_goal = self.goals.shape[0] - 1 # 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 This function is called at 60Hz """ meas = self.ros_interface.get_measurements() imu_meas = self.ros_interface.get_imu() self.meas = meas self.imu_meas = imu_meas # for computing motor gain v = 0.3 w = 0. self.ros_interface.command_velocity(v, w) return
class RobotControl(object): """ Robot interface class. In charge of getting sensor measurements (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 """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) self.diff_drive_controller = DiffDriveController(max_speed, max_omega) def process_measurements(self): """ Basic code to run autonomously """ # Check samples from https://github.com/markwsilliman/turtlebot/blob/master/goforward_and_avoid_obstacle.py sonar_meas = self.ros_interface.get_range() imu_meas = self.ros_interface.get_imu() if (sonar_meas == None): return elif (sonar_meas <= 30): pose = np.array([0, 0, math.pi / 4]) # emulate goal = np.array([0, 0]) else: pose = np.array([0, 0, 0]) goal = np.array([1, 0]) vel = self.diff_drive_controller.compute_vel(pose, goal) self.vel = vel[0:2] self.ros_interface.command_velocity(vel[0], vel[1]) rospy.loginfo_throttle(5, "robot_control velocity:" + str(self.vel) + " sonar:" + str(sonar_meas)) #+" imu:"+str(imu_meas)) #rospy.loginfo("robot_control velocity:"+ str(self.vel)+" sonar:"+str(sonar_meas))#+" imu:"+str(imu_meas)) return
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.kalman_filter = KalmanFilter(world_map) print("INITSTATE", self.kalman_filter.state) self.diff_drive_controller = DiffDriveController(max_speed, max_omega) self.vel = 0 self.omega = 0 self.curInd = 0 self.path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init, pos_goal) print(self.path) self.curGoal = self.path[0] self.done = False
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 """ # 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 This function is called at 60Hz """ meas = self.ros_interface.get_measurements() imu_meas = self.ros_interface.get_imu() #print imu_meas #self.ros_interface.command_velocity(0., -8.) if meas and meas != None: state=np.array([meas[0][0],meas[0][1],meas[0][2]]) vw=self.diff_drive_controller.compute_vel(state,np.array([[0,0]])) print vw while vw[2]: self.ros_interface.command_velocity(0,0) self.ros_interface.command_velocity(vw[0],vw[1])
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) """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) self.kalman_filter = KalmanFilter(world_map) self.prev_v = 0 self.est_pose = np.array([[0], [0], [0]]) self.diff_drive_controller = DiffDriveController(max_speed, max_omega) self.prev_imu_meas = np.array([[0], [0], [0], [0], [0]])
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 = []
class AmclAuxLocalization(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, camera_frame_id, base_frame_id, odom_frame_id, map_frame_id, ns_tag_detector, tag_pose_id, pos_init): """ Initialize the class """ # Handles all the ROS related items self.ros_interface = ROSInterface(camera_frame_id, base_frame_id, odom_frame_id, map_frame_id, ns_tag_detector) self.time = rospy.get_time() # YOUR CODE AFTER THIS #-------------------------------------------# self.markers = tag_pose_id self.idx_target_marker = 0 #-------------------------------------------# # Kalman filter self.kalman_filter = KalmanFilter(tag_pose_id) self.kalman_filter.mu_est = pos_init # 3*pos_init # For test def process_measurements(self): """ YOUR CODE HERE This function is called at 10Hz """ # tag_measurement = self.ros_interface.get_measurements() # tag_measurement = self.ros_interface.get_measurements_tf() tag_measurement = self.ros_interface.get_measurements_tf_directMethod() # amcl_pose = self.ros_interface.get_amcl_pose() print '-----------------' print 'tag:' print tag_measurement #----------------------------------------# # self.time = rospy.get_time() # print "self.time",self.time # Kalman filter # self.kalman_filter.step_filter(self.controlOut[0], (-1)*imu_meas, tag_measurement, rospy.get_time()) self.kalman_filter.step_filter_by_amcl(self.ros_interface, tag_measurement, rospy.get_time()) """ print "After--" print "mu_est",self.kalman_filter.mu_est print "angle_est =", (self.kalman_filter.mu_est[2,0]*180.0/np.pi), "deg" """ # return
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 self.markers = 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) # self.total_goals = self.goals.shape[0] # self.cur_goal = 2 # self.end_goal = self.goals.shape[0] - 1 # Uncomment as completed #self.kalman_filter = KalmanFilter(world_map) self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
class RobotControl(object): """ Robot interface """ 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.time_init = -1.0 def process_measurements(self): """ This function is called at 60Hz """ # set init time, and start motor #if 0 == 1: if self.time_init == -1.0: print "set init time" self.time_init = rospy.get_time() self.time_measurement = rospy.get_time() # stop motor when 1 second has passed diff_time = (self.time_measurement - self.time_init) #print("init: {}, measurement: {}, diff: {}".format(self.time_init, self.time_measurement, diff_time)) if diff_time >= 1: print "stop speed" self.ros_interface.command_velocity(0, 0) else: print "set speed" self.ros_interface.command_velocity(0.3, 0) return
def __init__(self, camera_frame_id, base_frame_id, odom_frame_id, map_frame_id, ns_tag_detector, tag_pose_id, pos_init): """ Initialize the class """ # Handles all the ROS related items self.ros_interface = ROSInterface(camera_frame_id, base_frame_id, odom_frame_id, map_frame_id, ns_tag_detector) self.time = rospy.get_time() # YOUR CODE AFTER THIS #-------------------------------------------# self.markers = tag_pose_id self.idx_target_marker = 0 #-------------------------------------------# # Kalman filter self.kalman_filter = KalmanFilter(tag_pose_id) self.kalman_filter.mu_est = pos_init # 3*pos_init # For test
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 """ # 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 This function is called at 60Hz """ meas = self.ros_interface.get_measurements() imu_meas = self.ros_interface.get_imu() #print(meas) done = False #print("time = " + str(self.robot_sim.last_meas_time)) if meas is not None and meas != []: print "meas = " + str(meas[0][0:3]) #print type(meas) state = -np.array(meas[0][0:3]) goal = np.array([0, 0]) print "state = " + str(state) #print type(state) #print goal #print type(goal) v, omega, done = self.diff_drive_controller.compute_vel( state, goal) #print done #self.robot_sim.command_velocity(v, omega) self.ros_interface.command_velocity(v, omega) else: #print "No measurement" #self.robot_sim.command_velocity(0, 0) v = 0 omega = 0 self.ros_interface.command_velocity(v, omega) print "v = " + str(v) print "omega = " + str(omega) return
class RobotControl(object): """ Robot interface class. In charge of getting sensor measurements (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 """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) def publish_action(self, speed, turn): self.ros_interface.command_velocity(speed, turn) def apply_avoidance_rules(self, distance_from_center, angle): p_angle = util.proportional_rotation(angle) action = "None" if self.inside_too_close_box(distance_from_center): # dangeorous distance, too close to turn, just try to go back action = "Back" rospy.loginfo("{}:{:.2f} {:.2f} dist:{:.2f}".format(action, angle, p_angle, distance_from_center)) self.react_to_inside_too_close_box(p_angle) elif util.is_forward_left(angle): # obstacle to left, go right action = "go forward right" self.publish_action(default_speed, p_angle) elif util.is_forward_right(angle): # obstacle to right, go left action = "go forward left" self.publish_action(default_speed, p_angle) elif util.is_backwards_left(angle): action = "obstacle backwards left" elif util.is_backwards_right(angle): action = "obstacle backwards right" else: action = "unknown" #rospy.loginfo("{}:{:.2f} {:.2f} dist:{:.2f}".format(action, angle, p_angle, distance_from_center)) def react_to_inside_too_close_box(self,angle): # go back and turn 180 degrees self.publish_action(-default_speed*2, 0) rate = rospy.Rate(avoid_check_rate) rate.sleep() if util.is_forward_right(angle): sign = -1 else: sign = 1 z = math.pi*1.6*sign rospy.loginfo('Back angle:'+str(angle)+' new angle:'+str(z)) self.publish_action(0, z) # around 5 rad/s rate.sleep() def inside_too_close_box(self, distance_from_center): forward_box_length, forward_box_width = self.forward_box() return distance_from_center < forward_box_width def get_closest_obstacle(self, scan): # returns closest point to obstacle inside a vehicle's 'forward box' # forward box is a rectangle ahead of the vehicle that if there is an obstacle # inside it it's necessary to do an action in order to avoid it forward_box_length, forward_box_width = self.forward_box() is_inside = False min_distance = max_distance min_i = 0 min_angle = 0 for i, distance in enumerate(scan.ranges): angle = scan.angle_min + i * scan.angle_increment if util.is_within_range(angle) and util.is_valid_distance(distance): #print i, distance, to_degrees(angle), angle if distance < min_distance: if point_is_inside_box(i, distance, scan, forward_box_length, forward_box_width, util.to_degrees(angle)): is_inside = True min_distance = distance min_i = i min_angle = angle #angle = angle_from_scan(min_i, scan) #angle_degrees = angle*180/math.pi #print min_i, angle_degrees, min_distance return is_inside, min_distance, min_angle def forward_box(self): # returns the Vehicle 'forward box' # to do: in future versions forward box should depend on vehicle's speed # more speed -> larger forward box length = Behaviour.body_width*look_ahead_factor width = clearance_safety_factor*Behaviour.body_width/2 return length, width def check_action(): pass def process_measurements(self): """ Basic code to run autonomously """ scan = self.ros_interface.get_scan() if(scan == None): return action = "None" angular_z = 0 linear_x = 0.3 is_inside, distance_from_center, angle = self.get_closest_obstacle(scan) # only do something if obstacle is inside forward box if is_inside: #print 'point inside ', distance_from_center, angle self.apply_avoidance_rules(distance_from_center, angle) else: self.publish_action(linear_x, angular_z) 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 # 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 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 """ print(' ') # 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() # meas is the position of the robot with respect to the AprilTags # print(meas) # now that we have the measurements, update the predicted state self.kalman_filter.step_filter(self.v, imu_meas, meas) # print(self.kalman_filter.x_t) # TODO remove on bot, shows predicted state on simulator #self.robot_sim.set_est_state(self.kalman_filter.x_t) # pull the next path point from the list cur_goal = self.getCarrot() # cur_goal = self.path[self.path_idx] # TODO test to just go to a goal # cur_goal[0] = 0.43 # cur_goal[1] = 2 # calculate the control commands need to reach next path point #print('') #print('current goal:') #print(cur_goal) #print('current state:') #print(self.kalman_filter.x_t) control_cmd = self.diff_drive_controller.compute_vel( self.kalman_filter.x_t, cur_goal) self.v = control_cmd[0] self.omega = control_cmd[1] #print('control command:') # print(control_cmd) if self.mission_complete: self.v = 0 self.omega = 0 #print(control_cmd) if control_cmd[2]: if len(self.path) > (self.path_idx + 1): self.path_idx = self.path_idx + 1 print('next goal') else: self.mission_complete = True #TODO calibration test on bot only for linear velocity ''' if self.start_time - 0 < 0.0001: self.start_time = rospy.get_time() if(rospy.get_time() - self.start_time > 4): self.v = 0 self.omega = 0 else: self.v = 0.15 self.omega = 0 ''' #TODO on bot only self.ros_interface.command_velocity(self.v, self.omega) #TODO for simulation #self.robot_sim.command_velocity(self.v,self.omega) return def getCarrot(self): ''' getCarrot - generates an artificial goal location along a path out infront of the robot. path - the set of points which make up the waypoints in the path position - the current position of the robot ''' path = self.path idx = self.path_idx pos = self.kalman_filter.x_t # if the current line segment ends in the goal point, set that to the goal if self.path_idx + 1 == len(self.path): return self.path[(len(self.path) - 1)] else: # find the point on the current line closest to the robot # calculate current line's slope and intercept pt1 = self.path[self.path_idx] pt2 = self.path[self.path_idx + 1] x_diff = pt2[0] - pt1[0] y_diff = pt2[1] - pt1[1] vert = abs(x_diff) < 0.001 # using the current line's slope and intercept find the point on that # line closest to the robots current point # assumes all lines are either veritcal or horizontal x_bot = pos[0][0] y_bot = pos[1][0] if vert: x_norm = pt2[0] y_norm = y_bot else: x_norm = x_bot y_norm = pt2[1] # if the normal point is past the end point of this segment inc path idx # assumes all lines are either vertical or horizontal inc = False if vert: if (y_diff > 0 and y_norm > pt2[1]) or (y_diff < 0 and y_norm < pt2[1]): inc = True else: if (x_diff > 0 and x_norm > pt2[0]) or (x_diff < 0 and x_norm < pt2[0]): inc = True if (inc): self.path_idx = self.path_idx + 1 print('increment path index') # find a point L distance infront of the normal point on this line # assumes all lines are either vertical or horizontal if vert: x_goal = pt2[0] if y_diff > 0: y_goal = y_bot + self.carrot_distance else: y_goal = y_bot - self.carrot_distance else: y_goal = pt2[1] if x_diff > 0: x_goal = x_bot + self.carrot_distance else: x_goal = x_bot - self.carrot_distance goal = np.array([x_goal, y_goal]) #print(goal) return 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) """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) self.kalman_filter = KalmanFilter(world_map) self.prev_v = 0 self.est_pose = np.array([[0], [0], [0]]) self.diff_drive_controller = DiffDriveController(max_speed, max_omega) self.prev_imu_meas = np.array([[0], [0], [0], [0], [0]]) def process_measurements(self, waypoint): """ waypoint is a 1D list [x,y] containing the next waypoint the rover needs to go YOUR CODE HERE Main loop of the robot - where all measurements, control, and esimtaiton are done. This function is called at 60Hz """ #This gives the xy location and the orientation of the tag in the rover frame #The orientation is zero when the rover faces directly at the tag meas = self.ros_interface.get_measurements() self.est_pose = self.kalman_filter.step_filter(self.prev_v, self.prev_imu_meas, meas) state = [self.est_pose[0, 0], self.est_pose[1, 0], self.est_pose[2, 0]] goal = [waypoint[0], waypoint[1]] controls = self.diff_drive_controller.compute_vel(state, goal) self.ros_interface.command_velocity(controls[0], controls[1]) self.prev_v = controls[0] imu_meas = self.ros_interface.get_imu() if imu_meas != None: imu_meas[3, 0] = -imu_meas[ 3, 0] #clockwise angular vel is positive from IMU self.prev_imu_meas = imu_meas return controls[2]
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 """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) # YOUR CODE AFTER THIS self.markers = 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) # self.total_goals = self.goals.shape[0] # self.cur_goal = 2 # self.end_goal = self.goals.shape[0] - 1 # 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 This function is called at 60Hz """ meas = self.ros_interface.get_measurements() imu_meas = self.ros_interface.get_imu() self.meas = meas self.imu_meas = imu_meas # unit test for following april tag if (meas != None and meas): cur_meas = meas[0] # print(cur_meas) tag_robot_pose = cur_meas[:3] # print(self.tag_pos(cur_meas[3])) tag_world_pose = np.float32(self.tag_pos(cur_meas[3])) # print(tag_world_pose) # print(tag_robot_pose) state = self.robot_pos(tag_world_pose,tag_robot_pose) goal = tag_world_pose v,w,done = self.diff_drive_controller.compute_vel(state,goal) if done: v = 0.01 w = 0. vel = (v,w) self.vel = vel[:2] self.done = done else: v = 0. w = 0. self.ros_interface.command_velocity(v,w) return def tag_pos(self, marker_id): # print(marker_id) # print(self.markers) for i in range(len(self.markers)): marker_i = np.copy(self.markers[i]) if int(float(marker_i[3])) == marker_id: return marker_i[:3] return None def robot_pos(self, w_pos, r_pos): H_W = np.array([[np.cos(w_pos[2]), -np.sin(w_pos[2]), w_pos[0]], [np.sin(w_pos[2]), np.cos(w_pos[2]), w_pos[1]], [0., 0., 1.]]) H_R = np.array([[np.cos(r_pos[2]), -np.sin(r_pos[2]), r_pos[0]], [np.sin(r_pos[2]), np.cos(r_pos[2]), r_pos[1]], [0., 0., 1.]]) # print(np.linalg.inv(H_R)) w_r = np.dot(H_W, np.linalg.inv(H_R)) return np.array([[w_r[0,2]],[w_r[1,2]],[np.arctan2(w_r[1,0],w_r[0,0])]])
class RobotControl(object): """ Robot interface class. In charge of getting sensor measurements (ROS subscribers), and transforms them into the 2D plane, and publishes velocity commands. """ def __init__(self, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body, min_lin_x, max_lin_x, rotate_fast_value, linear_k, k, min_linear_vel): """ Initialize the class """ self.min_lin_x = min_lin_x self.max_lin_x = max_lin_x self.rotate_fast_value = rotate_fast_value self.linear_k = linear_k self.k = k self.min_linear_vel = min_linear_vel # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) def to_degrees(self, rad): angle_degrees = rad * 180 / math.pi return angle_degrees def is_valid_distance(self, d): """ Discard Inf distances """ if (np.isnan(d) or np.isinf(d)): return False else: return True def is_within_range(self, i): """ Only consider -90 to 90 degrees """ #return -math.pi/4 < i < math.pi/4 return math.pi / 2 < abs(i) # LIDAR rotated backwards def process_measurements(self): """ Basic code to run autonomously """ # Check samples from https://github.com/markwsilliman/turtlebot/blob/master/goforward_and_avoid_obstacle.py scan = self.ros_interface.get_scan() linear = 0 rotational = 0 rcount = 0 hack = False msg = "" if (scan == None): return rcount = 0 for i, real_dist in enumerate(scan.ranges): angle = scan.angle_min + i * scan.angle_increment denom = 1. + real_dist * real_dist if self.is_within_range(angle) and self.is_valid_distance( real_dist): linear += math.sin(angle) / denom * 1. rotational += math.cos(angle) / denom * 1. rcount += 1 #print "%.5f"%self.to_degrees(angle)+"degrees x:%.5f"%real_dist+" x:%.2f"%linear+" z:%.2f"%rotational try: linear /= rcount rotational /= rcount except Exception as e: pass """ if (linear > self.k): linear = self.k elif (linear < -self.k): linear = -self.k """ linear_x = linear angular_z = rotational linear_x = linear # check this print "linear_x:%.5f" % linear_x + " angular_z:%.1f" % self.to_degrees( angular_z) + "(%.2f)" % angular_z + " " + msg #self.ros_interface.command_velocity(linear_x, angular_z) 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, 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!"
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 """ # 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 process_measurements(self): """ YOUR CODE HERE This function is called at 60Hz """ meas = self.ros_interface.get_measurements() self.meas = meas; print 'tag output' print meas # imu_meas: measurment comig from the imu imu_meas = self.ros_interface.get_imu() self.imu_meas = imu_meas print 'imu measurement' print imu_meas pose = self.kalman_filter.step_filter(self.vel, self.imu_meas, np.asarray(self.meas)) # 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(state, goal) self.vel = vel[0:2]; print vel if(not vel[2]): self.ros_interface.command_velocity(vel[0], vel[1]) else: vel = (0.01, 0.1) self.vel = vel self.ros_interface.command_velocity(vel[0], vel[1]) ''' # Code to move autonomously goal = self.goals[self.cur_goal] print 'pose' print pose print 'goal' print goal vel = self.diff_drive_controller.compute_vel(pose, goal) self.vel = vel[0:2]; print 'speed' print vel if(not vel[2]): self.ros_interface.command_velocity(vel[0], vel[1]) else: vel = (0, 0) if self.cur_goal < self.end_goal: self.cur_goal = self.cur_goal + 1 self.ros_interface.command_velocity(vel[0], vel[1]) self.vel = vel 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 """ # 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 process_measurements(self): """ YOUR CODE HERE This function is called at 60Hz """ meas = self.ros_interface.get_measurements() imu_meas = self.ros_interface.get_imu() # print 'meas',meas print 'imu_meas', imu_meas """ # Control the robot to track the tag if (meas is None) or (meas == []): # stop # self.ros_interface.command_velocity(0.0,0.0) # if self.count_noMeasurement > 30: # Actually the ros_interface will stop the motor itself if we don't keep sending new commands self.ros_interface.command_velocity(0.0,0.0) else: # Keep the old command self.ros_interface.command_velocity(self.controlOut[0],self.controlOut[1]) self.count_noMeasurement += 1 else: print 'meas',meas self.count_noMeasurement = 0 # Thew differential drive controller that lead the robot to the tag self.controlOut = self.diff_drive_controller.compute_vel((-1)*np.array(meas[0][0:3]),np.array([0.0,0.0,0.0])) self.ros_interface.command_velocity(self.controlOut[0],self.controlOut[1]) (v,omega,done) = (self.controlOut[0], self.controlOut[1],False ) """ """ if (rospy.get_time() - self.time) < 1.0: self.ros_interface.command_velocity(0.0, 3.0) # Linear velocity = 0.3 m/s, Angular velocity = 0.5 rad/s else: self.ros_interface.command_velocity(0.0,0.0) """ """ (v,omega,done) = (0.0, 0.0, False) self.ros_interface.command_velocity(v, omega) """ """ # Directly move to the goal position if self.controlOut[2]: # done self.ros_interface.command_velocity(0.0, 0.0) else: self.controlOut = self.wayPointFollowing.compute_vel(self.kalman_filter.mu_est, self.pos_goal ) # self.controlOut = self.diff_drive_controller.compute_vel(self.kalman_filter.mu_est, self.pos_goal ) if self.controlOut[2]: # done self.ros_interface.command_velocity(0.0, 0.0) else: self.ros_interface.command_velocity(self.controlOut[0], self.controlOut[1]) """ """ #----------------------------------------# # Switch the targets (way-point of the optimal path) and do the position control # if self.controlOut[2] and self.idx_path == self.size_path-1: # all done if self.idx_path == self.size_path-1 and self.controlOut[0] < 0.05 and self.controlOut[1] < 0.1 : # all done self.ros_interface.command_velocity(0.0, 0.0) else: self.controlOut = self.diff_drive_controller.compute_vel(self.kalman_filter.mu_est, self.path[self.idx_path,:] ) self.ros_interface.command_velocity(self.controlOut[0], self.controlOut[1]) if self.controlOut[2] and self.idx_path < self.size_path-1: # way-point done self.idx_path += 1 #----------------------------------------# """ #----------------------------------------# print "self.idx_path", self.idx_path # Switch the targets (way-point of the optimal path) and do the position control # way-point following (no reducing speeed when reach a way-point) # if self.controlOut[2] and self.idx_path == self.size_path-1: # all done if self.task_done: # all done self.ros_interface.command_velocity(0.0, 0.0) elif self.idx_path == self.size_path - 1: # The last one # Change the threshold self.diff_drive_controller.threshold = 0.02 # 3 cm # Using diff_drive_controller to reach the goal position with right direction self.controlOut = self.diff_drive_controller.compute_vel( self.kalman_filter.mu_est, self.path[self.idx_path, :]) # if self.controlOut[0] < 0.05 and self.controlOut[1] < 0.1 : # all done if self.controlOut[2]: # all done self.ros_interface.command_velocity(0.0, 0.0) self.task_done = True # all done else: self.ros_interface.command_velocity(self.controlOut[0], self.controlOut[1]) else: # The way-points, using wayPointFollowing to trace the trajectory without pausing if self.idx_path == self.size_path - 2: # The last 2nd one self.controlOut = self.diff_drive_controller.compute_vel( self.kalman_filter.mu_est, self.path[self.idx_path, :]) else: self.controlOut = self.wayPointFollowing.compute_vel( self.kalman_filter.mu_est, self.path[self.idx_path, :]) # self.controlOut = self.wayPointFollowing.compute_vel(self.kalman_filter.mu_est, self.path[self.idx_path,:] ) self.ros_interface.command_velocity(self.controlOut[0], self.controlOut[1]) if self.controlOut[ 2] and self.idx_path < self.size_path - 1: # way-point done self.idx_path += 1 #----------------------------------------# # self.time = rospy.get_time() # print "self.time",self.time # Kalman filter self.kalman_filter.step_filter(self.controlOut[0], (-1) * imu_meas, meas, rospy.get_time()) print "mu_est", self.kalman_filter.mu_est 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 """ # 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 This function is called at 60Hz """ meas = self.ros_interface.get_measurements( ) #type list (matriz de 1x3 meas[0][2], posicion del tag [[x, y, z, id]]) imu_meas = self.ros_interface.get_imu() if meas != None: # convert meas into a np array measu = meas[0] # IMPORTANTE USAR ***MEASU*** Y NO MEAS!!! #print measurements, python 2 btw... print("*******") print("Tag: "), print(measu[3]) measu3f = np.round(measu, 3) print("Measurements: "), print(measu3f[:3]) #print state state = np.array([0.0, 0.0, 0.0]) print("State: "), print(state) goal = np.array([measu[0], -measu[1], measu[2]]) vw = self.diff_drive_controller.compute_vel(state, goal) #print comanded velocity vw3f = np.round(vw, 3) print("Computed command vel: "), print(vw3f[:2]) if vw[2] == False: self.ros_interface.command_velocity(vw[0], vw[1]) if vw[2] == True: print("*****The robot arrived its destination*****") else: #self.ros_interface.command_velocity(0, 0) return
class RobotControl(object): """ Robot interface class. In charge of getting sensor measurements (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 """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) def to_degrees(self, rad): angle_degrees = rad * 180 / math.pi return angle_degrees def is_valid_distance(self, d): """ Discard Inf distances """ if (np.isnan(d) or np.isinf(d)): return False else: return True def is_within_range(self, i): """ Only consider -90 to 90 degrees """ #return -math.pi/4 < i < math.pi/4 return math.pi / 2 < abs(i) # LIDAR rotated backwards def process_measurements(self): """ Basic code to run autonomously """ # Check samples from https://github.com/Arkapravo/laser_obstacle_avoidance_pr2/blob/master/src/obstacle_avoidance_pr2.cpp scan = self.ros_interface.get_scan() angular_z = 0 linear_x = 0 k = 0.4 # 0.3 min_linear = 0.09 rotate_fast_value = 1. # 5.8 # rad/s linear_k = 0.15 go_forward = 0.5 # default value action = "None" if (scan == None): return #print "i:%.5f"%angle+"(%.5f"%self.to_degrees(angle)+"degrees) x:%.5f"%real_dist+" x:%.2f"%linear+" z:%.2f"%rotational min_range = scan.ranges[0] min_range_angle = 0 min_angle = 0 for j in range(0, 360): #increment by one degree real_dist = scan.ranges[j] angle = scan.angle_min + j * scan.angle_increment if self.is_within_range(angle) and self.is_valid_distance( real_dist): if (real_dist < min_range): min_range = real_dist min_angle = angle min_range_angle = j / 2 #print "minimum range is [%.3f]"%min_range+" at an angle of [%.3f]"%min_range_angle if ( min_range <= 0.5 ): # min_range<=0.5 gave box pushing like behaviour, min_range<=1.2 gave obstacle avoidance if (min_range_angle < 90): angular_z = rotate_fast_value linear_x = 0 action = "left " else: angular_z = -rotate_fast_value linear_x = 0 action = "right" else: angular_z = 0 linear_x = go_forward action = "straight" print "action:" + action + " min distance[%.2f]" % min_range + " angle [%.2f]" % min_range_angle + " (%.1f)" % angle + " min angle %.1f" % min_angle self.ros_interface.command_velocity(linear_x, angular_z) 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, pos_init, pos_goal, max_speed, max_omega): """ Initialize the class """ # Handles all the ROS related items self.ros_interface = ROSInterface() time.sleep(0.2) self.max_speed = max_speed self.max_omega = max_omega self.prev_lin_vel = 0.0 self.prev_ang_vel = 0.0 self.pos_init = pos_init self.pos_goal = pos_goal #self.joy = xbox.Joystick() #Initialize joystick self.state = [pos_init[0], pos_init[1], 0.0] #Kalman initialization self.kalman_filter = KalmanFilter(dim_x=3, dim_z=3) self.kalman_filter.F = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # State Transition Matrix self.kalman_filter.H = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # Measurement function self.kalman_filter.P = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]]) # covariance matrix self.kalman_filter.R = np.array([[0.035, 0, 0], [0, 0.035, 0], [0, 0, 0.001]]) # state uncertainty self.kalman_filter.Q = np.array([[0.005, 0, 0], [0, 0.005, 0], [0, 0, 0.008]]) # process uncertainty self.kalman_filter.x = np.array([self.state]).T #Uncomment as completed #self.diff_drive_controller = DiffDriveController(max_speed, max_omega) def KalmanUpdate(self): self.kalman_filter.predict() #For the P matrix prediction self.kalman_filter.x = np.array([self.state]).T self.kalman_filter.update(np.array( [MeasState[2:5]]).T) #MeasState[2:5] are the sensor data updated_state = [ self.kalman_filter.x[0][0], self.kalman_filter.x[1][0], self.kalman_filter.x[2][0] ] return updated_state def control(self): global ang_vel_rep_left, ang_vel_rep_right, period, baseAngle, baseMagValue, baseAccValue, MeasState global loop_ctr, AccList distances = self.ros_interface.get_distances() imu_meas = self.ros_interface.get_imu() #imu measurements MagValue = imu_meas[3:6] AccValue = [ imu_meas[0] - baseAccValue[0], imu_meas[1] - baseAccValue[1] ] #Skip acceleration noise if abs(AccValue[0]) < 0.15: AccValue[0] = 0.0 if abs(AccValue[1]) < 0.15: AccValue[1] = 0.0 Angle = -(math.atan2(MagValue[1], MagValue[0]) - baseAngle) #print([MagValue[0], MagValue[1]]) #State estimation from measurements MeasState[0] = (self.prev_lin_vel + AccValue[0] * period) * math.cos( self.state[2]) #x #Velocity MeasState[1] = (self.prev_lin_vel + AccValue[0] * period) * math.sin( self.state[2]) #y MeasState[2] = self.state[0] + MeasState[ 0] * period #Position mhpws 8elei self.state anti gia measstate? MeasState[3] = self.state[1] + MeasState[1] * period MeasState[4] = Angle #Predict next state self.state[0] = self.state[0] + self.prev_lin_vel * period * math.cos( self.state[2]) # x self.state[1] = self.state[1] + self.prev_lin_vel * period * math.sin( self.state[2]) # y self.state[2] = self.state[2] + self.prev_ang_vel * period # theta #self.state[2] = Angle #Updated state, sensor fusion self.state = self.KalmanUpdate() #self.state[2] = (self.KalmanUpdate())[2] #Angle between -pi and pi if self.state[2] > math.pi: self.state[2] -= 2 * math.pi elif self.state[2] < -math.pi: self.state[2] += 2 * math.pi print(self.state[0], self.state[1], self.state[2] * 180 / math.pi) #Repultion fields from sensors ang_vel_rep_left = max(0.0, (self.max_omega / 2 - 1.0 * distances[0]) * 3.0) ang_vel_rep_right = -max( 0.0, (self.max_omega / 2 - 1.0 * distances[2]) * 3.0) angle_from_goal = math.atan2(self.pos_goal[1] - self.state[1], self.pos_goal[0] - self.state[0]) #Attraction field if abs(angle_from_goal - self.state[2]) < 0.1: ang_vel_attr = 0 else: ang_vel_attr = 2.3 * (angle_from_goal - self.state[2]) ang_vel_attr = np.sign(ang_vel_attr) * min( 0.7, abs(ang_vel_attr)) #angle_vel between -0.7 and 0.7 #print(ang_vel_attr) lin_vel = min(self.max_speed, 5 * (distances[1] - 0.16)) #Linear velocity not below 0.1 m/s if lin_vel > 0: lin_vel = max(lin_vel, 0.1) else: lin_vel = min(lin_vel, -0.1) lin_vel = max(lin_vel, -self.max_speed) ang_vel = ang_vel_attr + ang_vel_rep_right + ang_vel_rep_left self.ros_interface.command_velocity(lin_vel, ang_vel) self.prev_lin_vel = lin_vel self.prev_ang_vel = ang_vel return def PerformSquare(self): global baseAngle, MeasState, square_angles, square_index, timer imu_meas = self.ros_interface.get_imu() #imu measurements MagValue = imu_meas[3:5] Angle = -(math.atan2(MagValue[1], MagValue[0]) - baseAngle) self.state[2] = Angle if self.state[2] > math.pi: #angle between -pi and pi self.state[2] -= 2 * math.pi elif self.state[2] < -math.pi: self.state[2] += 2 * math.pi if (square_index == 2) and (self.state[2] < 0): self.state[2] += 2 * math.pi print(self.state[2] * 180 / math.pi) if timer > 3: timer = -1 square_index += 1 angle_from_goal = square_angles[square_index] #Attraction field if abs(angle_from_goal - self.state[2]) < 0.06: ang_vel_attr = 0 else: ang_vel_attr = 2.5 * (angle_from_goal - self.state[2]) ang_vel_attr = np.sign(ang_vel_attr) * min( 1.5, abs(ang_vel_attr)) #angle_vel between -1.2 and 1.2 self.ros_interface.command_velocity(0.2, ang_vel_attr) return