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
