class AnalyzePhase(picamera.array.PiMotionAnalysis): def __init__(self, camera, bridge): picamera.array.PiMotionAnalysis.__init__(self, camera) self.bridge = bridge self.br = tf.TransformBroadcaster() # bind method calls to subscribed topics rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback) rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback) rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback) rospy.Subscriber("/pidrone/infrared", Range, self.range_callback) rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback) self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1) self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True) self.lr_pid = PIDaxis(10.0, 0.000, 0.0, midpoint=0, control_range=(-5.0, 5.0)) self.fb_pid = PIDaxis(10.0, 0.000, 0.0, midpoint=0, control_range=(-5.0, 5.0)) self.detector = cv2.ORB(nfeatures=NUM_FEATURES, scoreType=cv2.ORB_FAST_SCORE ) # FAST_SCORE is a little faster to compute map_grid_kp, map_grid_des = create_map('map.jpg') self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des) self.first_locate = True self.first_hold = True self.prev_img = None self.prev_kp = None self.prev_des = None self.locate_position = False self.prev_time = None self.prev_rostime = None self.pos = [0, 0, 0] self.yaw = 0.0 self.z = 0.075 self.iacc_yaw = 0.0 self.hold_position = False self.target_pos = [0, 0, 0, 0] self.target_yaw = 0.0 self.map_counter = 0 self.max_map_counter = 0 self.mode = Mode() self.mode.mode = 5 # constant self.kp_yaw = 50.0 self.ki_yaw = 0.1 self.alpha_yaw = 0.1 # perceived yaw smoothing alpha self.hybrid_alpha = 0.3 # blend position with first frame and int # angle self.angle_x = 0.0 # the hz of state_controller is different self.angle_y = 0.0 def write(self, data): curr_img = np.reshape(np.fromstring(data, dtype=np.uint8), (CAMERA_HEIGHT, CAMERA_WIDTH, 3)) curr_rostime = rospy.Time.now() curr_time = curr_rostime.to_sec() # start MCL localization if self.locate_position: curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None) if curr_kp is not None and curr_kp is not None: # generate particles for the first time if self.first_locate: particle = self.estimator.initialize_particles( NUM_PARTICLE, curr_kp, curr_des) self.first_locate = False self.pos = [ particle.x(), particle.y(), particle.z(), particle.yaw() ] self.yaw = particle.yaw() print 'first', particle else: # get a estimate velocity over time particle = self.estimator.update(self.z, self.angle_x, self.angle_y, self.prev_kp, self.prev_des, curr_kp, curr_des) # update position self.pos = [ self.hybrid_alpha * particle.x() + (1.0 - self.hybrid_alpha) * self.pos[0], self.hybrid_alpha * particle.y() + (1.0 - self.hybrid_alpha) * self.pos[1], self.z ] self.yaw = self.alpha_yaw * particle.yaw() + ( 1.0 - self.alpha_yaw) * self.yaw # print 'particle', particle print '--pose', self.pos[0], self.pos[1], self.yaw # if average particle weight is close to initial weight if is_almost_equal(particle.weight(), PROB_THRESHOLD): self.map_counter = self.map_counter - 1 elif self.map_counter <= 0: self.map_counter = 1 else: self.map_counter = min(self.map_counter + 1, -MAX_BAD_COUNT) # if it's been a while without a high average particle weight if self.map_counter < MAX_BAD_COUNT: self.first_locate = True self.fb_pid._i = 0 self.lr_pid._i = 0 self.iacc_yaw = 0.0 self.map_counter = 0 self.mode.x_velocity = 0 self.mode.y_velocity = 0 self.mode.yaw_velocity = 0 self.pospub.publish(self.mode) print 'Restart localization' else: if self.hold_position: if self.first_hold: self.target_pos = self.pos self.target_yaw = 0 # rotate is not implement self.first_hold = False image_message = self.bridge.cv2_to_imgmsg( curr_img, encoding="bgr8") self.first_image_pub.publish(image_message) else: err_x = self.target_pos[0] - self.pos[0] err_y = self.target_pos[1] - self.pos[1] self.mode.x_velocity = self.lr_pid.step( err_x, curr_time - self.prev_time) self.mode.y_velocity = self.fb_pid.step( err_y, curr_time - self.prev_time) err_yaw = self.target_yaw - self.yaw self.iacc_yaw += err_yaw * self.ki_yaw self.mode.yaw_velocity = err_yaw * self.kp_yaw + self.iacc_yaw self.pospub.publish(self.mode) print '--target', self.target_pos[ 0], self.target_pos[1], self.target_yaw print 'count', self.map_counter else: print "CANNOT FIND ANY FEATURES !!!!!" self.prev_kp = curr_kp self.prev_des = curr_des self.prev_img = curr_img self.prev_time = curr_time self.prev_rostime = curr_rostime self.br.sendTransform( (self.pos[0], self.pos[1], self.z), tf.transformations.quaternion_from_euler(0, 0, self.yaw), rospy.Time.now(), "base", "world") # update angle data when '/pidrone/angle' is published to def angle_callback(self, data): self.angle_x = data.twist.angular.x self.angle_y = data.twist.angular.y # update z when '/pidrone/infrared' is published to def range_callback(self, data): if data.range != -1: self.z = data.range # start localization when '/pidrone/reset_transform' is published to (press 'r') def reset_callback(self, data): print "Start localization" self.locate_position = True self.first_locate = True self.hold_position = False self.map_counter = 0 self.max_map_counter = 0 # toggle position hold when '/pidrone/toggle_transform' is published to (press 'p') def toggle_callback(self, data): self.hold_position = not self.hold_position self.first_hold = True self.fb_pid._i = 0 self.lr_pid._i = 0 self.iacc_yaw = 0.0 print "Position hold", "enabled." if self.hold_position else "disabled." # called whenever '/pidrone/set_mode' is published to, velocity mode is default def mode_callback(self, data): self.mode.mode = data.mode if not self.hold_position or data.mode == 4 or data.mode == 3: print "VELOCITY" # TODO scale is not consistent, check index.html and pid_class.py data.z_velocity = data.z_velocity * 100 self.pospub.publish(data) else: self.target_pos[0] += data.x_velocity / 100. self.target_pos[1] += data.y_velocity / 100. print "Target position", self.target_pos
class AnalyzePhase(picamera.array.PiMotionAnalysis): def __init__(self, camera, bridge): picamera.array.PiMotionAnalysis.__init__(self, camera) self.bridge = bridge self.br = tf.TransformBroadcaster() rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback) rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback) rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback) rospy.Subscriber("/pidrone/infrared", Range, self.range_callback) self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1) self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True) self.lr_pid = PIDaxis(4.00, -0.00000, 0.000, midpoint=0, control_range=(-10.0, 10.0)) self.fb_pid = PIDaxis(4.00, 0.0000, -0.000, midpoint=0, control_range=(-10.0, 10.0)) self.prev_img = None self.detector = cv2.ORB(nfeatures=120, scoreType=cv2.ORB_FAST_SCORE) self.index_params = dict(algorithm=6, table_number=6, key_size=12, multi_probe_level=1) self.search_params = dict(checks=50) self.matcher = cv2.FlannBasedMatcher(self.index_params, self.search_params) self.first_kp = None self.first_des = None self.prev_kp = None self.prev_des = None self.first = True self.lr_err = ERR() self.fb_err = ERR() self.prev_time = None self.prev_rostime = None self.pos = [0, 0] self.z = 0.075 self.smoothed_yaw = 0.0 self.yaw_observed = 0.0 self.iacc_yaw = 0.0 self.transforming = False self.last_first_time = None self.target_x = 0 self.target_y = 0 self.first_counter = 0 self.max_first_counter = 0 self.mode = Mode() # constant self.kp_yaw = 80.0 self.ki_yaw = 0.1 self.kpi_yaw = 15.0 self.kpi_max_yaw = 0.01 self.alpha_yaw = 0.1 # perceived yaw smoothing alpha self.hybrid_alpha = 0.1 # blend position with first frame and int def write(self, data): curr_img = np.reshape(np.fromstring(data, dtype=np.uint8), (240, 320, 3)) curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None) curr_rostime = rospy.Time.now() curr_time = curr_rostime.to_sec() if self.first: print "taking new first" self.first = False self.first_kp = curr_kp self.first_des = curr_des self.last_first_time = curr_time image_message = self.bridge.cv2_to_imgmsg(curr_img, encoding="bgr8") self.first_image_pub.publish(image_message) elif self.transforming: # move the drone to the right, x is negative. # move the drone to the front, y is positive. corr_first = self.feature_transform(self.first_kp, self.first_des, curr_kp, curr_des) if corr_first is not None: self.last_first_time = curr_time self.first_counter += 1 self.max_first_counter = max(self.first_counter, self.max_first_counter) # X and Y first_displacement = [-corr_first[0, 2] / 280., corr_first[1, 2] / 280.] self.pos = [ self.hybrid_alpha * first_displacement[0] * self.z + (1.0 - self.hybrid_alpha) * self.pos[0], self.hybrid_alpha * first_displacement[1] * self.z + (1.0 - self.hybrid_alpha) * self.pos[1]] self.lr_err.err = self.target_x - self.pos[0] self.fb_err.err = self.target_y - self.pos[1] print "ERR", self.lr_err.err, self.fb_err.err self.mode.x_i = self.lr_pid.step(self.lr_err.err, curr_time - self.prev_time) self.mode.y_i = self.fb_pid.step(self.fb_err.err, curr_time - self.prev_time) cvc_vel = 1.2 self.mode.x_velocity = cvc_vel * self.mode.x_i self.mode.y_velocity = cvc_vel * self.mode.y_i # Yaw scalex = np.linalg.norm(corr_first[:, 0]) scalez = np.linalg.norm(corr_first[:, 1]) corr_first[:, 0] /= scalex corr_first[:, 1] /= scalez self.yaw_observed = np.arctan2(corr_first[1, 0], corr_first[0, 0]) self.smoothed_yaw = (1.0 - self.alpha_yaw) * self.smoothed_yaw + self.alpha_yaw * self.yaw_observed yaw = self.smoothed_yaw self.iacc_yaw += yaw * self.ki_yaw yaw_kpi_term = np.sign(yaw) * yaw * yaw * self.kpi_yaw if abs(yaw_kpi_term) < self.kpi_max_yaw: self.iacc_yaw += yaw_kpi_term else: self.iacc_yaw += np.sign(yaw) * self.kpi_max_yaw self.mode.yaw_velocity = yaw * self.kp_yaw + self.iacc_yaw print "yaw iacc: ", self.iacc_yaw self.pospub.publish(self.mode) print "first", self.max_first_counter, self.first_counter else: self.first_counter = 0 corr_int = self.feature_transform(self.prev_kp, self.prev_des, curr_kp, curr_des) if corr_int is not None: time_since_first = curr_time - self.last_first_time print "integrated", time_since_first print "max_first_counter: ", self.max_first_counter # X and Y int_displacement = [-corr_int[0, 2] / 280., corr_int[1, 2] / 280.] self.pos[0] += int_displacement[0] * self.z self.pos[1] += int_displacement[1] * self.z self.lr_err.err = self.target_x - self.pos[0] self.fb_err.err = self.target_y - self.pos[1] print "ERR", self.lr_err.err, self.fb_err.err self.mode.x_i = self.lr_pid.step(self.lr_err.err, curr_time - self.prev_time) self.mode.y_i = self.fb_pid.step(self.fb_err.err, curr_time - self.prev_time) cvc_vel = 1.3 # 0.25 #1.0 self.mode.x_velocity = cvc_vel * self.mode.x_i self.mode.y_velocity = cvc_vel * self.mode.y_i # Yaw self.mode.yaw_velocity = self.iacc_yaw print "yaw iacc: ", self.iacc_yaw self.pospub.publish(self.mode) else: print "LOST" else: corr_first = self.feature_transform(self.first_kp, self.first_des, curr_kp, curr_des) if corr_first is not None: self.last_first_time = rospy.get_time() if curr_time - self.last_first_time > 2: self.first = True else: print "no first", curr_time - self.last_first_time self.prev_img = curr_img self.prev_kp = curr_kp self.prev_des = curr_des self.prev_time = curr_time self.prev_rostime = curr_rostime # TODO should be z instead of pos[2], which has never been updated self.br.sendTransform((self.pos[0], self.pos[1], self.z), tf.transformations.quaternion_from_euler(0, 0, self.yaw_observed), rospy.Time.now(), "base", "world") def feature_transform(self, kp1, des1, kp2, des2): transform = None if des1 is not None and des2 is not None: matches = self.matcher.knnMatch(des1, des2, k=2) good = [] for match in matches: if len(match) > 1 and match[0].distance < 0.7 * match[1].distance: good.append(match[0]) src_pts = np.float32([kp1[m.queryIdx].pt for m in good]).reshape(-1, 1, 2) dst_pts = np.float32([kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2) # estimateRigidTransform needs at least three pairs if src_pts is not None and dst_pts is not None and len(src_pts) > 3 and len(dst_pts) > 3: transform = cv2.estimateRigidTransform(src_pts, dst_pts, False) return transform def range_callback(self, data): if data.range != -1: self.z = data.range def reset_callback(self, data): print "Resetting Phase" self.first = True self.pos = [0, 0] self.fb_pid._i = 0 self.lr_pid._i = 0 self.target_x = 0 self.target_y = 0 self.smoothed_yaw = 0.0 self.iacc_yaw = 0.0 self.first_counter = 0 self.max_first_counter = 0 def toggle_callback(self, data): self.transforming = not self.transforming print "Position hold", "enabled." if self.transforming else "disabled." def mode_callback(self, data): if not self.transforming or data.mode == 4 or data.mode == 3: print "VELOCITY" self.pospub.publish(data) else: # TODO 4 is used for what ? Should the target be accumulated ? self.target_x = data.x_velocity * 4. self.target_y = data.y_velocity * 4. print "POSITION", self.target_x, self.target_y
class PIDController(object): ''' Controls the flight of the drone by running a PID controller on the error calculated by the desired and current velocity and position of the drone ''' def __init__(self): # Initialize the current and desired modes self.current_mode = Mode('DISARMED') self.desired_mode = Mode('DISARMED') # Initialize in velocity control self.position_control = False # Initialize the current and desired positions self.current_position = Position() self.desired_position = Position(z=0.3) self.last_desired_position = Position(z=0.3) # Initialize the position error self.position_error = Error() # Initialize the current and desired velocities self.current_velocity = Velocity() self.desired_velocity = Velocity() # Initialize the velocity error self.velocity_error = Error() # Set the distance that a velocity command will move the drone (m) self.desired_velocity_travel_distance = 0.1 # Set a static duration that a velocity command will be held self.desired_velocity_travel_time = 0.1 # Set a static duration that a yaw velocity command will be held self.desired_yaw_velocity_travel_time = 0.25 # Store the start time of the desired velocities self.desired_velocity_start_time = None self.desired_yaw_velocity_start_time = None # Initialize the primary PID self.pid = PID() # Initialize the error used for the PID which is vx, vy, z where vx and # vy are velocities, and z is the error in the altitude of the drone self.pid_error = Error() # Initialize the 'position error to velocity error' PIDs: # left/right (roll) pid self.lr_pid = PIDaxis(kp=10.0, ki=0.5, kd=5.0, midpoint=0, control_range=(-10.0, 10.0)) # front/back (pitch) pid self.fb_pid = PIDaxis(kp=10.0, ki=0.5, kd=5.0, midpoint=0, control_range=(-10.0, 10.0)) # Initialize the pose callback time self.last_pose_time = None # Initialize the desired yaw velocity self.desired_yaw_velocity = 0 # Initialize the current and previous roll, pitch, yaw values self.current_rpy = RPY() self.previous_rpy = RPY() # initialize the current and previous states self.current_state = State() self.previous_state = State() # Store the command publisher self.cmdpub = None # a variable used to determine if the drone is moving between disired # positions self.moving = False # a variable that determines the maximum magnitude of the position error # Any greater position error will overide the drone into velocity # control self.safety_threshold = 1.5 # determines if the position of the drone is known self.lost = False # determines if the desired poses are aboslute or relative to the drone self.absolute_desired_position = False # determines whether to use open loop velocity path planning which is # accomplished by calculate_travel_time self.path_planning = True # ROS SUBSCRIBER CALLBACK METHODS ################################# def current_state_callback(self, state): """ Store the drone's current state for calculations """ self.previous_state = self.current_state self.current_state = state self.state_to_three_dim_vec_structs() def desired_pose_callback(self, msg): """ Update the desired pose """ # store the previous desired position self.last_desired_position = self.desired_position # set the desired positions equal to the desired pose message if self.absolute_desired_position: self.desired_position.x = msg.position.x self.desired_position.y = msg.position.y # the desired z must be above z and below the range of the ir sensor (.55meters) self.desired_position.z = msg.position.z if 0 <= desired_z <= 0.5 else self.last_desired_position.z # set the desired positions relative to the current position (except for z to make it more responsive) else: self.desired_position.x = self.current_position.x + msg.position.x self.desired_position.y = self.current_position.y + msg.position.y # set the disired z position relative to the last desired position (doesn't limit the mag of the error) # the desired z must be above z and below the range of the ir sensor (.55meters) desired_z = self.last_desired_position.z + msg.position.z self.desired_position.z = desired_z if 0 <= desired_z <= 0.5 else self.last_desired_position.z if self.desired_position != self.last_desired_position: # the drone is moving between desired positions self.moving = True print 'moving' def desired_twist_callback(self, msg): """ Update the desired twist """ self.desired_velocity.x = msg.linear.x self.desired_velocity.y = msg.linear.y self.desired_velocity.z = msg.linear.z self.desired_yaw_velocity = msg.angular.z self.desired_velocity_start_time = None self.desired_yaw_velocity_start_time = None if self.path_planning: self.calculate_travel_time() def current_mode_callback(self, msg): """ Update the current mode """ self.current_mode = msg.mode def desired_mode_callback(self, msg): """ Update the desired mode """ self.desired_mode = msg.mode def position_control_callback(self, msg): """ Set whether or not position control is enabled """ self.position_control = msg.data if self.position_control: self.reset_callback(Empty()) print "Position Control", self.position_control def reset_callback(self, empty): """ Reset the desired and current poses of the drone and set desired velocities to zero """ self.current_position = Position(z=self.current_position.z) self.desired_position = Position(z=self.current_position.z) self.desired_velocity.x = 0 self.desired_velocity.y = 0 def lost_callback(self, msg): self.lost = msg.data # Step Method ############# def step(self): """ Returns the commands generated by the pid """ self.calc_error() if self.position_control: if self.position_error.planar_magnitude( ) < self.safety_threshold and not self.lost: if self.moving: if self.position_error.magnitude() > 0.05: self.pid_error -= self.velocity_error * 100 else: self.moving = False print 'not moving' else: self.position_control_pub.publish(False) if self.desired_velocity.magnitude() > 0 or abs( self.desired_yaw_velocity) > 0: self.adjust_desired_velocity() return self.pid.step(self.pid_error, self.desired_yaw_velocity) # HELPER METHODS ################ def state_to_three_dim_vec_structs(self): """ Convert the values from the state estimator into ThreeDimVec structs to make calculations concise """ # store the positions pose = self.current_state.pose_with_covariance.pose self.current_position.x = pose.position.x self.current_position.y = pose.position.y self.current_position.z = pose.position.z # store the linear velocities twist = self.current_state.twist_with_covariance.twist self.current_velocity.x = twist.linear.x self.current_velocity.y = twist.linear.y self.current_velocity.z = twist.linear.z # store the orientations self.previous_rpy = self.current_rpy quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) r, p, y = tf.transformations.euler_from_quaternion(quaternion) self.current_rpy = RPY(r, p, y) def adjust_desired_velocity(self): """ Set the desired velocity back to 0 once the drone has traveled the amount of time that causes it to move the specified desired velocity travel distance if path_planning otherwise just set the velocities back to 0 after the . This is an open loop method meaning that the specified travel distance cannot be guarenteed. If path planning_planning is false, just set the velocities back to zero, this allows the user to move the drone for as long as they are holding down a key """ curr_time = rospy.get_time() # set the desired planar velocities to zero if the duration is up if self.desired_velocity_start_time is not None: # the amount of time the set point velocity is not zero duration = curr_time - self.desired_velocity_start_time if duration > self.desired_velocity_travel_time: self.desired_velocity.x = 0 self.desired_velocity.y = 0 self.desired_velocity_start_time = None else: self.desired_velocity_start_time = curr_time # set the desired yaw velocity to zero if the duration is up if self.desired_yaw_velocity_start_time is not None: # the amount of time the set point velocity is not zero duration = curr_time - self.desired_yaw_velocity_start_time if duration > self.desired_yaw_velocity_travel_time: self.desired_yaw_velocity = 0 self.desired_yaw_velocity_start_time = None else: self.desired_yaw_velocity_start_time = curr_time def calc_error(self): """ Calculate the error in velocity, and if in position hold, add the error from lr_pid and fb_pid to the velocity error to control the position of the drone """ # store the time difference pose_dt = 0 if self.last_pose_time != None: pose_dt = rospy.get_time() - self.last_pose_time self.last_pose_time = rospy.get_time() # calculate the velocity error self.velocity_error = self.desired_velocity - self.current_velocity # calculate the z position error dz = self.desired_position.z - self.current_position.z # calculate the pid_error from the above values self.pid_error.x = self.velocity_error.x self.pid_error.y = self.velocity_error.y self.pid_error.z = dz # multiply by 100 to account for the fact that code was originally written using cm self.pid_error = self.pid_error * 100 if self.position_control: # calculate the position error self.position_error = self.desired_position - self.current_position # calculate a value to add to the velocity error based based on the # position error in the x (roll) direction lr_step = self.lr_pid.step(self.position_error.x, pose_dt) # calculate a value to add to the velocity error based based on the # position error in the y (pitch) direction fb_step = self.fb_pid.step(self.position_error.y, pose_dt) self.pid_error.x += lr_step self.pid_error.y += fb_step def calculate_travel_time(self): ''' return the amount of time that desired velocity should be used to calculate the error in order to move the drone the specified travel distance for a desired velocity ''' if self.desired_velocity.magnitude() > 0: # tiime = distance / velocity travel_time = self.desired_velocity_travel_distance / self.desired_velocity.planar_magnitude( ) else: travel_time = 0.0 self.desired_velocity_travel_time = travel_time def reset(self): ''' Set desired_position to be current position, set filtered_desired_velocity to be zero, and reset both the PositionPID and VelocityPID ''' # reset position control variables self.position_error = Error(0, 0, 0) self.desired_position = Position(self.current_position.x, self.current_position.y, 0.3) # reset velocity control_variables self.velocity_error = Error(0, 0, 0) self.desired_velocity = Velocity(0, 0, 0) # reset the pids self.pid.reset() self.lr_pid.reset() self.fb_pid.reset() def ctrl_c_handler(self, signal, frame): """ Gracefully handles ctrl-c """ print 'Caught ctrl-c\n Stopping Controller' sys.exit() def publish_cmd(self, cmd): """Publish the controls to /pidrone/fly_commands """ msg = RC() msg.roll = cmd[0] msg.pitch = cmd[1] msg.yaw = cmd[2] msg.throttle = cmd[3] self.cmdpub.publish(msg)
class AnalyzePhase: def __init__(self): self.bridge = CvBridge() self.br = tf.TransformBroadcaster() # self.lr_pid = PIDaxis(11.0, 0.001, 0.001, midpoint=0, control_range=(-5.0, 5.0)) # self.fb_pid = PIDaxis(11.0, 0.001, 0.001, midpoint=0, control_range=(-5.0, 5.0)) self.lr_pid = PIDaxis(9.0, 0.001, 0.001, midpoint=0, control_range=(-4.0, 4.0)) self.fb_pid = PIDaxis(9.0, 0.001, 0.001, midpoint=0, control_range=(-4.0, 4.0)) self.detector = cv2.ORB(nfeatures=200, scoreType=cv2.ORB_FAST_SCORE) # FAST_SCORE is a little faster to compute map_grid_kp, map_grid_des = create_map('big_map.jpg') self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des) self.first_locate = True self.first_hold = True self.prev_img = None self.prev_kp = None self.prev_des = None self.locate_position = False self.prev_time = None self.prev_rostime = None self.pos = [0, 0, 0] self.yaw = 0.0 self.z = 0.075 self.set_z = INIZ_Z self.iacc_yaw = 0.0 self.hold_position = False self.target_pos = [0, 0, self.set_z] self.target_point = Point() self.target_yaw = 0.0 self.map_counter = 0 self.max_map_counter = 0 self.mode = Mode() self.mode.mode = 5 # constant self.kp_yaw = 40.0 self.ki_yaw = 0.1 self.alpha_yaw = 0.1 # perceived yaw smoothing alpha self.hybrid_alpha = 0.75 # blend position with first frame and int # angle self.angle_x = 0.0 # the hz of state_controller is different self.angle_y = 0.0 # TODO: current z position is controlled by state_controller self.increment_z = 0 # path from MDP self.path = [] self.path_index = -1 rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback) rospy.Subscriber('/pidrone/path', Float32MultiArray, self.path_callback) rospy.Subscriber('/hololens/path', String, self.hololens_path_callback) rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback) rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback) rospy.Subscriber("/pidrone/capture_photo", Empty, self.capture_callback) rospy.Subscriber("/pidrone/infrared", Range, self.range_callback) rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback) rospy.Subscriber('/pidrone/picamera/image_raw', Image, self.image_callback) self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1) self.target_pos_pub = rospy.Publisher('/pidrone/drone_position', Point, queue_size=1) self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True) self.captured_image_pub = rospy.Publisher("/pidrone/picamera/captured_image", CompressedImage, queue_size=1, latch=True) def image_callback(self, data): curr_img = self.bridge.imgmsg_to_cv2(data, desired_encoding="passthrough") self.prev_img = curr_img curr_rostime = rospy.Time.now() curr_time = curr_rostime.to_sec() # process path from MDP if len(self.path) != 0 and is_close_to(self.pos, self.target_pos, distance=0.15): if self.path[self.path_index + 1] == -10: # take photo if is_close_to(self.pos, self.target_pos, distance=0.1): # image_message = self.bridge.cv2_to_imgmsg(curr_img, encoding="bgr8") # self.first_image_pub.publish(image_message) image_message = self.bridge.cv2_to_compressed_imgmsg(curr_img) self.captured_image_pub.publish(image_message) self.path_index += 1 else: # take action if self.increment_z == 0: self.target_pos[0] += self.path[self.path_index + 1] self.target_pos[1] += self.path[self.path_index + 2] self.increment_z = self.path[self.path_index + 3] * 100. self.target_pos[2] += self.path[self.path_index + 3] self.path_index += 3 if self.path_index == len(self.path) - 1: # reset path self.path = [] self.path_index = -1 # start MC localization if self.locate_position: curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None) if curr_kp is not None and curr_kp is not None: # generate particles for the first time if self.first_locate: particle = self.estimator.initialize_particles(NUM_PARTICLE, curr_kp, curr_des) self.first_locate = False self.pos = particle.position[0:3] self.pos[1] += 0.05 # camera offset self.yaw = particle.position[3] print 'first', particle else: # get a estimate velocity over time particle = self.estimator.update(self.z, self.angle_x, self.angle_y, self.prev_kp, self.prev_des, curr_kp, curr_des) # update position self.pos = [self.hybrid_alpha * particle.position[0] + (1.0 - self.hybrid_alpha) * self.pos[0], self.hybrid_alpha * particle.position[1] + (1.0 - self.hybrid_alpha) * self.pos[1], self.z] self.yaw = self.alpha_yaw * particle.position[3] + (1.0 - self.alpha_yaw) * self.yaw # print 'particle', particle print '--pose', str(round(self.pos[0], 3)), str(round(self.pos[1], 3)), str(round(self.pos[2], 3)), self.yaw # if all particles are not good estimations if is_almost_equal(particle.weight, PROB_THRESHOLD): self.map_counter = self.map_counter - 1 elif self.map_counter <= 0: self.map_counter = 1 else: self.map_counter = min(self.map_counter + 1, -MAX_BAD_COUNT) # if no particles are good estimations, we should restart if self.map_counter < MAX_BAD_COUNT: self.first_locate = True self.fb_pid._i = 0 self.lr_pid._i = 0 self.iacc_yaw = 0.0 self.map_counter = 0 self.mode.x_velocity = 0 self.mode.y_velocity = 0 self.mode.z_velocity = 0 self.mode.yaw_velocity = 0 self.pospub.publish(self.mode) print 'Restart localization' else: if self.hold_position: if self.first_hold: self.target_pos[0] = (int)(self.pos[0] / CELL_DISTANCE) * CELL_DISTANCE + CELL_DISTANCE / 2. self.target_pos[1] = (int)(self.pos[1] / CELL_DISTANCE) * CELL_DISTANCE + CELL_DISTANCE / 2. self.target_pos[2] = self.set_z self.target_yaw = 0 # rotate is not implement self.first_hold = False # image_message = self.bridge.cv2_to_imgmsg(curr_img, encoding="bgr8") # self.first_image_pub.publish(image_message) else: err_x = self.target_pos[0] - self.pos[0] err_y = self.target_pos[1] - self.pos[1] self.mode.x_velocity = self.lr_pid.step(err_x, curr_time - self.prev_time) self.mode.y_velocity = self.fb_pid.step(err_y, curr_time - self.prev_time) self.mode.z_velocity = self.increment_z self.increment_z = 0 err_yaw = self.target_yaw - self.yaw self.iacc_yaw += err_yaw * self.ki_yaw self.mode.yaw_velocity = err_yaw * self.kp_yaw + self.iacc_yaw self.pospub.publish(self.mode) self.target_point.x = self.target_pos[0] self.target_point.y = self.target_pos[1] self.target_point.z = self.target_pos[2] self.target_pos_pub.publish(self.target_point) print '--target', self.target_pos[0], self.target_pos[1], self.target_pos[2], self.target_yaw print 'count', self.map_counter else: print "CANNOT FIND ANY FEATURES !!!!!" self.prev_kp = curr_kp self.prev_des = curr_des self.prev_time = curr_time self.prev_rostime = curr_rostime self.br.sendTransform((self.pos[0], self.pos[1], self.z), tf.transformations.quaternion_from_euler(0, 0, self.yaw), rospy.Time.now(), "base", "world") # the angle is just estimate def angle_callback(self, data): #self.angle_x = data.twist.angular.x #self.angle_y = data.twist.angular.y self.angle_x = 0 self.angle_y = 0 def range_callback(self, data): if data.range != -1: self.z = data.range def reset_callback(self, data): print "Start localization" self.set_z = self.target_pos[2] self.path = [] self.path_index = -1 self.locate_position = True self.first_locate = True self.hold_position = False self.first_hold = True self.fb_pid._i = 0 self.lr_pid._i = 0 self.iacc_yaw = 0.0 self.map_counter = 0 self.max_map_counter = 0 def toggle_callback(self, data): self.hold_position = not self.hold_position self.first_hold = True self.fb_pid._i = 0 self.lr_pid._i = 0 self.iacc_yaw = 0.0 print "Position hold", "enabled." if self.hold_position else "disabled." def capture_callback(self, data): image_message = self.bridge.cv2_to_imgmsg(self.prev_img, encoding="bgr8") self.first_image_pub.publish(image_message) image_message = self.bridge.cv2_to_compressed_imgmsg(self.prev_img) self.captured_image_pub.publish(image_message) def mode_callback(self, data): if not self.hold_position or data.mode == 4 or data.mode == 3: print "VELOCITY" # TODO scale is not consistent, check index.html and pid_class.py data.z_velocity = data.z_velocity * 100. self.pospub.publish(data) else: self.target_pos[0] += data.x_velocity / 100. * 5 self.target_pos[1] += data.y_velocity / 100. * 5 # TODO scale is not consistent, check index.html and pid_class.py if self.increment_z == 0: self.target_pos[2] += data.z_velocity self.increment_z = data.z_velocity * 100. print "Target position", self.target_pos def path_callback(self, data): self.path = data.data print(self.path) def hololens_path_callback(self, data): self.path = [float(str(round(float(i), 2))) for i in (data.data).split()] print(self.path)
class AnalyzePhase(picamera.array.PiMotionAnalysis): def __init__(self, camera, bridge): picamera.array.PiMotionAnalysis.__init__(self, camera) self.bridge = bridge self.br = tf.TransformBroadcaster() rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback) rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback) rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback) rospy.Subscriber("/pidrone/infrared", Range, self.range_callback) rospy.Subscriber('/pidrone/angle_and_velocity', Twist, self.angle_and_velocity_callback) self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1) self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True) self.lr_pid = PIDaxis(10.0, 0.000, 1.0, midpoint=0, control_range=(-10.0, 10.0)) self.fb_pid = PIDaxis(10.0, 0.000, 1.0, midpoint=0, control_range=(-10.0, 10.0)) self.detector = cv2.ORB(nfeatures=120, scoreType=cv2.ORB_FAST_SCORE ) # FAST_SCORE is a little faster to compute map_imgs = [ cv2.imread('map1.jpg'), cv2.imread('map2.jpg'), cv2.imread('map3.jpg'), cv2.imread('map4.jpg') ] map_detector = cv2.ORB(nfeatures=500, scoreType=cv2.ORB_FAST_SCORE) # TODO use particle filter, so we can use more features which makes GridAdaptedFeatureDetector work better # map_detector = cv2.GridAdaptedFeatureDetector(self.detector, maxTotalKeypoints=500) # find features evenly self.map_index = 0 self.map_features = [] # save features for each map in an array for map_img in map_imgs: map_kp = map_detector.detect(map_img, None) map_kp, map_des = self.detector.compute(map_img, map_kp) if map_kp is None or map_des is None: print "Map cannot be detect and compute !" sys.exit() else: self.map_features.append((map_kp, map_des)) self.map_kp, self.map_des = self.map_features[0] self.prev_img = None index_params = dict(algorithm=6, table_number=6, key_size=12, multi_probe_level=1) search_params = dict(checks=50) self.matcher = cv2.FlannBasedMatcher(index_params, search_params) self.prev_kp = None self.prev_des = None self.first = True self.prev_time = None self.prev_rostime = None self.pos = [0, 0] self.z = 0.075 self.angle_x = 0.0 # the hz of state_controller is different self.angle_y = 0.0 self.smoothed_yaw = 0.0 self.yaw_observed = 0.0 self.iacc_yaw = 0.0 self.transforming = False self.target_pos = [0, 0] self.map_counter = 0 self.max_map_counter = 0 self.map_missing_counter = 0 self.mode = Mode() self.mode.mode = 5 # constant self.kp_yaw = 100.0 self.ki_yaw = 0.1 self.alpha_yaw = 0.1 # perceived yaw smoothing alpha self.hybrid_alpha = 0.2 # blend position with first frame and int def write(self, data): curr_img = np.reshape(np.fromstring(data, dtype=np.uint8), (CAMERA_HEIGHT, CAMERA_WIDTH, 3)) curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None) curr_rostime = rospy.Time.now() curr_time = curr_rostime.to_sec() if self.transforming: # cannot find matched features in current map, then change map if self.map_missing_counter > MAX_MISSING_COUNT: if self.map_index == len(self.map_features) - 1: self.map_index = 0 else: self.map_index += 1 self.map_kp, self.map_des = self.map_features[self.map_index] print 'change to map', self.map_index if self.first: pos, yaw = self.feature_locate(curr_kp, curr_des) if pos is not None and yaw is not None: self.map_missing_counter = 0 self.pos = pos self.yaw_observed = -yaw # take the opposite self.smoothed_yaw = -yaw # take the opposite self.target_pos = pos self.first = False image_message = self.bridge.cv2_to_imgmsg(curr_img, encoding="bgr8") self.first_image_pub.publish(image_message) print "start and set pose", pos else: self.map_missing_counter += 1 print 'start or reset failed' else: pos, yaw = self.feature_locate(curr_kp, curr_des) if pos is not None and yaw is not None: self.map_counter += 1 self.max_map_counter = max(self.map_counter, self.max_map_counter) self.map_missing_counter = 0 self.pos = [ self.hybrid_alpha * pos[0] + (1.0 - self.hybrid_alpha) * self.pos[0], self.hybrid_alpha * pos[1] + (1.0 - self.hybrid_alpha) * self.pos[1] ] err_x = self.target_pos[0] - self.pos[0] err_y = self.target_pos[1] - self.pos[1] self.mode.x_velocity = self.lr_pid.step( err_x, curr_time - self.prev_time) self.mode.y_velocity = self.fb_pid.step( err_y, curr_time - self.prev_time) self.yaw_observed = -yaw # take the opposite self.smoothed_yaw = ( 1.0 - self.alpha_yaw ) * self.smoothed_yaw + self.alpha_yaw * self.yaw_observed yaw = self.smoothed_yaw self.iacc_yaw += yaw * self.ki_yaw self.mode.yaw_velocity = yaw * self.kp_yaw + self.iacc_yaw self.pospub.publish(self.mode) print "MAP", self.max_map_counter, self.map_counter, "\nPOS", self.pos, "\nTARGET", self.target_pos else: # TODO or hover ? self.map_counter = 0 self.map_missing_counter += 1 err_x = self.target_pos[0] - self.pos[0] err_y = self.target_pos[1] - self.pos[1] self.mode.x_velocity = self.lr_pid.step( err_x, curr_time - self.prev_time) / 10. # less power self.mode.y_velocity = self.fb_pid.step( err_y, curr_time - self.prev_time) / 10. # less power yaw = self.smoothed_yaw self.iacc_yaw += yaw * self.ki_yaw self.mode.yaw_velocity = ( yaw * self.kp_yaw + self.iacc_yaw) / 10. # less power print "LOST !", "\nPOS", self.pos, "\nTARGET", self.target_pos self.prev_img = curr_img self.prev_kp = curr_kp self.prev_des = curr_des self.prev_time = curr_time self.prev_rostime = curr_rostime self.br.sendTransform( (self.pos[0], self.pos[1], self.z), tf.transformations.quaternion_from_euler(0, 0, self.yaw_observed), rospy.Time.now(), "base", "world") def feature_transform(self, kp1, des1, kp2, des2): transform = None if des1 is not None and des2 is not None: matches = self.matcher.knnMatch(des1, des2, k=2) good = [] for match in matches: if len(match ) > 1 and match[0].distance < 0.7 * match[1].distance: good.append(match[0]) src_pts = np.float32([kp1[m.queryIdx].pt for m in good]).reshape(-1, 1, 2) dst_pts = np.float32([kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2) # estimateRigidTransform needs at least three pairs if src_pts is not None and dst_pts is not None and len( src_pts) > 3 and len(dst_pts) > 3: transform = cv2.estimateRigidTransform(src_pts, dst_pts, False) return transform def feature_locate(self, kp, des): """ find features from current image, match features with map, compute the location. :param kp: train kp :param des: train des :return: global x, y, and yaw """ pos = None yaw = None if des is not None: matches = self.matcher.knnMatch(des, self.map_des, k=2) # take from opencv tutorial good = [] for match in matches: if len(match ) > 1 and match[0].distance < 0.7 * match[1].distance: good.append(match[0]) if len(good) > MIN_MATCH_COUNT: src_pts = np.float32([kp[m.queryIdx].pt for m in good]).reshape(-1, 1, 2) dst_pts = np.float32([ self.map_kp[m.trainIdx].pt for m in good ]).reshape(-1, 1, 2) transform = cv2.estimateRigidTransform(src_pts, dst_pts, False) if transform is not None: transformed_center = cv2.transform( CAMERA_CENTER, transform) # get local pixel transformed_center = [ transformed_center[0][0][0] / float(METER_TO_PIXEL), # map to local pose (MAP_HEIGHT - transformed_center[0][0][1]) / float(METER_TO_PIXEL) ] transformed_center = self.local_to_global( transformed_center) # get the global pose yaw = np.arctan2(transform[1, 0], transform[0, 0]) # get global heading # correct the pose if the drone is not level z = math.sqrt(self.z**2 / (1 + math.tan(self.angle_x)**2 + math.tan(self.angle_y)**2)) offset_x = np.tan(self.angle_x) * z offset_y = np.tan(self.angle_y) * z global_offset_x = math.cos(yaw) * offset_x + math.sin( yaw) * offset_y global_offset_y = math.sin(yaw) * offset_x + math.cos( yaw) * offset_y pos = [ transformed_center[0] + global_offset_x, transformed_center[1] + global_offset_y ] # else: # print "Not enough matches are found - %d/%d" % (len(good), MIN_MATCH_COUNT) return pos, yaw def local_to_global(self, pos): if self.map_index == 1: pos[1] += MAP_REAL_HEIGHT elif self.map_index == 2: pos[0] += MAP_REAL_WIDTH pos[1] += MAP_REAL_HEIGHT elif self.map_index == 3: pos[0] += MAP_REAL_WIDTH return pos def angle_and_velocity_callback(self, data): self.angle_x = data.angular.x self.angle_y = data.angular.y def range_callback(self, data): if data.range != -1: self.z = data.range def reset_callback(self, data): print "Resetting position" self.first = True self.fb_pid._i = 0 self.lr_pid._i = 0 self.iacc_yaw = 0.0 self.map_counter = 0 self.max_map_counter = 0 self.map_missing_counter = 0 self.map_index = 0 self.map_kp, self.map_des = self.map_features[0] def toggle_callback(self, data): self.transforming = not self.transforming print "Position hold", "enabled." if self.transforming else "disabled." def mode_callback(self, data): if not self.transforming or data.mode == 4 or data.mode == 3: print "VELOCITY" # TODO scale is not consistent, check index.html and pid_class.py data.z_velocity = data.z_velocity * 100 self.pospub.publish(data) else: self.target_pos[0] += data.x_velocity / 100. self.target_pos[1] += data.y_velocity / 100. print "POSITION", self.target_pos
class AnalyzePhase: def __init__(self): self.bridge = CvBridge() self.br = tf.TransformBroadcaster() self.lr_pid = PIDaxis(12.5, 0.001, 0.0023, midpoint=0, control_range=(-5.0, 5.0)) self.fb_pid = PIDaxis(12.5, 0.001, 0.0023, midpoint=0, control_range=(-5.0, 5.0)) self.detector = cv2.ORB(nfeatures=500, scoreType=cv2.ORB_FAST_SCORE ) # FAST_SCORE is a little faster to compute map_grid_kp, map_grid_des = create_map('map.jpg') self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des) self.first_locate = True self.first_hold = True self.prev_img = None self.prev_kp = None self.prev_des = None self.locate_position = False self.prev_time = None self.prev_rostime = None self.pos = [0, 0, 0] self.yaw = 0.0 self.z = 0.075 self.iacc_yaw = 0.0 self.hold_position = False self.target_pos = [0, 0, 0] self.target_yaw = 0.0 self.map_counter = 0 self.max_map_counter = 0 self.mode = Mode() self.mode.mode = 5 # constant self.kp_yaw = 50.0 self.ki_yaw = 0.1 self.alpha_yaw = 0.1 # perceived yaw smoothing alpha self.hybrid_alpha = 0.3 # blend position with first frame and int # angle self.angle_x = 0.0 # the hz of state_controller is different self.angle_y = 0.0 rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback) rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback) rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback) rospy.Subscriber("/pidrone/infrared", Range, self.range_callback) rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback) rospy.Subscriber('/pidrone/picamera/image_raw', Image, self.image_callback) self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1) self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True) def image_callback(self, data): curr_img = self.bridge.imgmsg_to_cv2(data, desired_encoding="passthrough") curr_rostime = rospy.Time.now() curr_time = curr_rostime.to_sec() # start MC localization if self.locate_position: curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None) if curr_kp is not None and curr_kp is not None: # generate particles for the first time if self.first_locate: particle = self.estimator.initialize_particles( NUM_PARTICLE, curr_kp, curr_des) self.first_locate = False self.pos = particle.position[0:3] self.yaw = particle.position[3] print 'first', particle else: # get a estimate velocity over time particle = self.estimator.update(self.z, self.angle_x, self.angle_y, self.prev_kp, self.prev_des, curr_kp, curr_des) # update position self.pos = [ self.hybrid_alpha * particle.position[0] + (1.0 - self.hybrid_alpha) * self.pos[0], self.hybrid_alpha * particle.position[1] + (1.0 - self.hybrid_alpha) * self.pos[1], self.z ] self.yaw = self.alpha_yaw * particle.position[3] + ( 1.0 - self.alpha_yaw) * self.yaw # print 'particle', particle print '--pose', self.pos[0], self.pos[1], self.yaw # if all particles are not good estimations if is_almost_equal(particle.weight, PROB_THRESHOLD): self.map_counter = self.map_counter - 1 elif self.map_counter <= 0: self.map_counter = 1 else: self.map_counter = min(self.map_counter + 1, -MAX_BAD_COUNT) # if no particles are good estimations, we should restart if self.map_counter < MAX_BAD_COUNT: self.first_locate = True self.fb_pid._i = 0 self.lr_pid._i = 0 self.iacc_yaw = 0.0 self.map_counter = 0 self.mode.x_velocity = 0 self.mode.y_velocity = 0 self.mode.yaw_velocity = 0 self.pospub.publish(self.mode) print 'Restart localization' else: if self.hold_position: if self.first_hold: self.target_pos = self.pos self.target_yaw = 0 # rotate is not implement self.first_hold = False image_message = self.bridge.cv2_to_imgmsg( curr_img, encoding="bgr8") self.first_image_pub.publish(image_message) else: err_x = self.target_pos[0] - self.pos[0] err_y = self.target_pos[1] - self.pos[1] self.mode.x_velocity = self.lr_pid.step( err_x, curr_time - self.prev_time) self.mode.y_velocity = self.fb_pid.step( err_y, curr_time - self.prev_time) err_yaw = self.target_yaw - self.yaw self.iacc_yaw += err_yaw * self.ki_yaw self.mode.yaw_velocity = err_yaw * self.kp_yaw + self.iacc_yaw self.pospub.publish(self.mode) print '--target', self.target_pos[ 0], self.target_pos[1], self.target_yaw print 'count', self.map_counter else: print "CANNOT FIND ANY FEATURES !!!!!" self.prev_kp = curr_kp self.prev_des = curr_des self.prev_time = curr_time self.prev_rostime = curr_rostime self.br.sendTransform( (self.pos[0], self.pos[1], self.z), tf.transformations.quaternion_from_euler(0, 0, self.yaw), rospy.Time.now(), "base", "world") # the angle is just estimate def angle_callback(self, data): self.angle_x = data.twist.angular.x self.angle_y = data.twist.angular.y def range_callback(self, data): if data.range != -1: self.z = data.range def reset_callback(self, data): print "Start localization" self.locate_position = True self.first_locate = True self.hold_position = False self.map_counter = 0 self.max_map_counter = 0 def toggle_callback(self, data): self.hold_position = not self.hold_position self.first_hold = True self.fb_pid._i = 0 self.lr_pid._i = 0 self.iacc_yaw = 0.0 print "Position hold", "enabled." if self.hold_position else "disabled." def mode_callback(self, data): if not self.hold_position or data.mode == 4 or data.mode == 3: print "VELOCITY" # TODO scale is not consistent, check index.html and pid_class.py data.z_velocity = data.z_velocity * 100 self.pospub.publish(data) else: self.target_pos[0] += data.x_velocity / 100. self.target_pos[1] += data.y_velocity / 100. print "Target position", self.target_pos
class AnalyzePhase(picamera.array.PiMotionAnalysis): def __init__(self, camera, bridge): picamera.array.PiMotionAnalysis.__init__(self, camera) self.bridge = bridge self.br = tf.TransformBroadcaster() rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback) rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback) rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback) rospy.Subscriber("/pidrone/infrared", Range, self.range_callback) rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback) rospy.Subscriber('/pidrone/plane_err', TwistStamped, self.picam_velocity_callback) self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1) self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True) self.lr_pid = PIDaxis(20.0, 0.000, 2.0, midpoint=0, control_range=(-10.0, 10.0)) self.fb_pid = PIDaxis(20.0, 0.000, 2.0, midpoint=0, control_range=(-10.0, 10.0)) self.detector = cv2.ORB(nfeatures=100, scoreType=cv2.ORB_FAST_SCORE ) # FAST_SCORE is a little faster to compute map_grid_kp, map_grid_des = create_map('map.jpg') self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des) self.first_locate = True self.first_hold = True self.prev_img = None self.locate_position = False self.prev_velocity_time = None self.prev_time = None self.prev_rostime = None self.pos = [0, 0, 0] self.yaw = 0.0 self.z = 0.075 self.iacc_yaw = 0.0 self.hold_position = False self.target_pos = [0, 0, 0] self.target_yaw = 0.0 self.map_counter = 0 self.max_map_counter = 0 self.mode = Mode() self.mode.mode = 5 # constant self.kp_yaw = 70.0 self.ki_yaw = 0.1 self.alpha_yaw = 0.1 # perceived yaw smoothing alpha self.hybrid_alpha = 0.2 # blend position with first frame and int # angle, velocity self.angle_x = 0.0 # the hz of state_controller is different self.angle_y = 0.0 self.angle_prev_time = None self.mw_angle_comp_x = 0.0 self.mw_angle_comp_y = 0.0 self.velocities_x = [] self.velocities_y = [] self.velocities_z = [] self.velocities_yaw = [] self.alpha_yaw = 0.1 # perceived yaw smoothing alpha self.hybrid_alpha = 0.2 # blend position with first frame and int def write(self, data): curr_img = np.reshape(np.fromstring(data, dtype=np.uint8), (CAMERA_HEIGHT, CAMERA_WIDTH, 3)) curr_rostime = rospy.Time.now() curr_time = curr_rostime.to_sec() # start MCL localization if self.locate_position: curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None) if curr_kp is not None and curr_kp is not None: # generate particles for the first time if self.first_locate: particle = self.estimator.initialize_particles( NUM_PARTICLE, curr_kp, curr_des) self.first_locate = False self.pos = particle.position[0:3] self.yaw = particle.position[3] print 'first', particle else: # get a estimate velocity over time velocity = [ np.average(self.velocities_x), np.average(self.velocities_y), np.average(self.velocities_z), np.average(self.velocities_yaw) ] self.velocities_x = [] self.velocities_y = [] self.velocities_z = [] self.velocities_yaw = [] if self.prev_velocity_time is None: self.prev_velocity_time = curr_time curr_velocity_time = time.time() delta_time = curr_velocity_time - self.prev_velocity_time self.prev_velocity_time = curr_velocity_time particle = self.estimator.update(self.z, self.angle_x, self.angle_y, velocity, delta_time, curr_kp, curr_des) # update position self.pos = [ self.hybrid_alpha * particle.position[0] + (1.0 - self.hybrid_alpha) * self.pos[0], self.hybrid_alpha * particle.position[1] + (1.0 - self.hybrid_alpha) * self.pos[1], self.z ] self.yaw = self.alpha_yaw * particle.position[3] + ( 1.0 - self.alpha_yaw) * self.yaw print 'particle', particle # if all particles are not good estimations if is_almost_equal(particle.weight, PROB_THRESHOLD): self.map_counter = self.map_counter - 1 elif self.map_counter <= 0: self.map_counter = 1 else: self.map_counter = min(self.map_counter + 1, -MAX_BAD_COUNT) print 'count', self.map_counter # if no particles are good estimations, we should restart if self.map_counter < MAX_BAD_COUNT: self.first_locate = True self.map_counter = 0 print 'Restart localization' else: if self.hold_position: if self.first_hold: self.target_pos = self.pos self.target_yaw = 0 # rotate is not implement self.first_hold = False image_message = self.bridge.cv2_to_imgmsg( curr_img, encoding="bgr8") self.first_image_pub.publish(image_message) else: err_x = self.target_pos[0] - self.pos[0] err_y = self.target_pos[1] - self.pos[1] self.mode.x_velocity = self.lr_pid.step( err_x, curr_time - self.prev_time) self.mode.y_velocity = self.fb_pid.step( err_y, curr_time - self.prev_time) err_yaw = self.target_yaw - self.yaw self.iacc_yaw += err_yaw * self.ki_yaw self.mode.yaw_velocity = err_yaw * self.kp_yaw + self.iacc_yaw self.pospub.publish(self.mode) print 'pose', self.pos, self.yaw, '\ntarget', self.target_pos, self.target_yaw else: print "CANNOT FIND ANY FEATURES !!!!!" self.prev_img = curr_img self.prev_time = curr_time self.prev_rostime = curr_rostime self.br.sendTransform( (self.pos[0], self.pos[1], self.z), tf.transformations.quaternion_from_euler(0, 0, self.yaw), rospy.Time.now(), "base", "world") # the angle is just estimate def angle_callback(self, data): # take from state_controller.py curr_time = data.header.stamp.to_sec() if self.angle_prev_time is None: self.angle_prev_time = curr_time self.mw_angle_comp_x = np.tan( (data.twist.angular.x - self.angle_x) * (curr_time - self.angle_prev_time)) * 10.0 self.mw_angle_comp_y = np.tan( (data.twist.angular.y - self.angle_y) * (curr_time - self.angle_prev_time)) * 10.0 self.angle_x = data.twist.angular.x self.angle_y = data.twist.angular.y self.angle_prev_time = curr_time # assume go forward is positive and rotate to right is positive def picam_velocity_callback(self, data): self.velocities_x.append( -(data.twist.linear.x - self.mw_angle_comp_x) * self.z * VELOCITY_X_SCALE) self.velocities_y.append( -(data.twist.linear.y + self.mw_angle_comp_y) * self.z * VELOCITY_Y_SCALE) self.velocities_z.append(0.0) self.velocities_yaw.append(-data.twist.angular.z * VELOCITY_YAW_SCALE) def range_callback(self, data): if data.range != -1: self.z = data.range def reset_callback(self, data): print "Start localization" self.locate_position = True self.first_locate = True self.hold_position = False self.map_counter = 0 self.max_map_counter = 0 self.velocities_x = [] self.velocities_y = [] self.velocities_z = [] self.velocities_yaw = [] self.prev_velocity_time = None def toggle_callback(self, data): self.hold_position = not self.hold_position self.first_hold = True self.fb_pid._i = 0 self.lr_pid._i = 0 self.iacc_yaw = 0.0 print "Position hold", "enabled." if self.hold_position else "disabled." def mode_callback(self, data): if not self.hold_position or data.mode == 4 or data.mode == 3: print "VELOCITY" # TODO scale is not consistent, check index.html and pid_class.py data.z_velocity = data.z_velocity * 100 self.pospub.publish(data) else: self.target_pos[0] += data.x_velocity / 100. self.target_pos[1] += data.y_velocity / 100. print "Target position", self.target_pos