def servo_to_tag(self, pose_goal, goal_error=[0.03, 0.03, 0.1], initial_ar_pose=None): lost_tag_thresh = 0.6 #0.4 # TODO REMOVE # err_pub = rospy.Publisher("servo_err", Float32MultiArray) # if False: # self.test_move() # return "aborted" ####################### goal_ar_pose = homo_mat_from_2d(*pose_goal) rate = 8. kf_x = ServoKalmanFilter(delta_t=1./rate) kf_y = ServoKalmanFilter(delta_t=1./rate) kf_r = ServoKalmanFilter(delta_t=1./rate) if initial_ar_pose is not None: ar_err = homo_mat_to_2d(homo_mat_from_2d(*initial_ar_pose) * goal_ar_pose**-1) kf_x.update(ar_err[0]) kf_y.update(ar_err[1]) kf_r.update(ar_err[2]) print "initial_ar_pose", initial_ar_pose pid_x = PIDController(k_p=0.75, rate=rate, saturation=0.05) pid_y = PIDController(k_p=0.75, rate=rate, saturation=0.05) pid_r = PIDController(k_p=0.75, rate=rate, saturation=0.08) r = rospy.Rate(rate) self.preempt_requested = False while True: if rospy.is_shutdown(): self.base_pub.publish(Twist()) return 'aborted' if self.preempt_requested: self.preempt_requested = False self.base_pub.publish(Twist()) return 'preempted' goal_mkr = create_base_marker(goal_ar_pose, 0, (0., 1., 0.)) self.mkr_pub.publish(goal_mkr) if self.cur_ar_pose is not None: # make sure we have a new observation new_obs = self.ar_pose_updated self.ar_pose_updated = False # find the error between the AR tag and goal pose print "self.cur_ar_pose", self.cur_ar_pose cur_ar_pose_2d = homo_mat_from_2d(*homo_mat_to_2d(self.cur_ar_pose)) print "cur_ar_pose_2d", cur_ar_pose_2d ar_mkr = create_base_marker(cur_ar_pose_2d, 1, (1., 0., 0.)) self.mkr_pub.publish(ar_mkr) ar_err = homo_mat_to_2d(cur_ar_pose_2d * goal_ar_pose**-1) print "ar_err", ar_err print "goal_ar_pose", goal_ar_pose # filter this error using a Kalman filter x_filt_err, x_filt_cov, x_unreli = kf_x.update(ar_err[0], new_obs=new_obs) y_filt_err, y_filt_cov, y_unreli = kf_y.update(ar_err[1], new_obs=new_obs) r_filt_err, r_filt_cov, r_unreli = kf_r.update(ar_err[2], new_obs=new_obs) print ([x_unreli, y_unreli, r_unreli]) if np.any(np.array([x_unreli, y_unreli, r_unreli]) > [lost_tag_thresh*3]): #moved the bracket to the right to be around the 3 self.base_pub.publish(Twist()) print "Why is this here.....???" return 'lost_tag' print "Noise:", x_unreli, y_unreli, r_unreli # TODO REMOVE #ma = Float32MultiArray() #ma.data = [x_filt_err[0,0], x_filt_err[1,0], ar_err[0], # x_unreli, y_unreli, r_unreli] #err_pub.publish(ma) print "xerr" print x_filt_err print x_filt_cov print "Cov", x_filt_cov[0,0], y_filt_cov[0,0], r_filt_cov[0,0] x_ctrl = pid_x.update_state(x_filt_err[0,0]) y_ctrl = pid_y.update_state(y_filt_err[0,0]) r_ctrl = pid_r.update_state(r_filt_err[0,0]) base_twist = Twist() base_twist.linear.x = x_ctrl base_twist.linear.y = y_ctrl base_twist.angular.z = r_ctrl cur_filt_err = np.array([x_filt_err[0,0], y_filt_err[0,0], r_filt_err[0,0]]) print "err", ar_err print "Err filt", cur_filt_err print "Twist:", base_twist if np.all(np.fabs(cur_filt_err) < goal_error): self.base_pub.publish(Twist()) return 'succeeded' self.base_pub.publish(base_twist) r.sleep()