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()
