class ArucoNavigation(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, tag_poses, poselist_base2cam, pose_init, wheelbase,
wheel_radius):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
pos_init - (3,) Numpy array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - (3,) Numpy array specifying the final position of the robot,
also formatted as (x,y,theta)
"""
# Initialize BaseController object: sends velocity commands and receives odometry measurements from Arduino
# self._base_controller = BaseController()
# Initialize wheel_angle subscriber
self._wheel_angle = dict()
self._wheel_angle['left'] = 0
self._wheel_angle['right'] = 0
self._wheel_angle_lock = threading.Lock()
rospy.Subscriber("/wheel_angle", WheelAngle, self.wheel_angle_callback)
# Initialize AprilTagLocalization object: broadcasts the static pose transforms
# and calculates the base_link pose in map frame from on Apriltag detections
self._aruco_localization = ArucoLocalization(tag_poses,
poselist_base2cam)
# Initialize subscriber to Apriltag measurements
# rospy.Subscriber("/tag_pose_array", PoseArray, tag_pose_callback)
self._kalman_filter = KalmanFilter(pose_init, wheelbase, wheel_radius)
self._diff_drive_controller = DiffDriveController()
# Initialize cmd_vel publisher
self._cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
self._br = tf.TransformBroadcaster()
def wheel_angle_callback(self, msg):
with self._wheel_angle_lock:
self._wheel_angle['left'] = msg.left
self._wheel_angle['right'] = msg.right
def process_measurements(self, goal):
"""
YOUR CODE HERE
Main loop of the robot - where all measurements, control, and estimation
are done.
"""
self._aruco_localization.broadcast_static_tf()
# odom_meas = self._base_controller.get_measurement() # current wheel angles (wheel_angle_left, wheel_angle_right)
tag_meas = self._aruco_localization.get_measurement(
) # calculated robot pose(s) based on Apriltag detections (x, y, theta)
with self._wheel_angle_lock:
odom_meas = [self._wheel_angle['left'], self._wheel_angle['right']]
# Debug
# print 'wheel_angle_left = ', self._base_controller._wheel_angle_left
# print 'wheel_angle_right = ', self._base_controller._wheel_angle_right
# print 'odom_meas = ', odom_meas
# print 'tag_meas = ', tag_meas
# odom_meas = None
# tag_meas = None
# Do KalmanFilter step
pose_est = self._kalman_filter.step_filter(odom_meas, tag_meas)
# print 'pose_est = ', pose_est
# print 'goal = ', goal
poselist_map2base_ekf = [
pose_est[0], pose_est[1], 0, 0, 0, pose_est[2]
]
pubFrame(self._br,
pose=poselist_map2base_ekf,
frame_id='/base_link_ekf',
parent_frame_id='/map')
at_goal = False
v, omega, at_goal = self._diff_drive_controller.compute_vel(
pose_est, goal)
cmd_vel_msg = Twist()
cmd_vel_msg.linear.x = v
cmd_vel_msg.angular.z = omega
# self._cmd_vel_pub.publish(cmd_vel_msg)
# self._base_controller.command_velocity(v, omega)
return at_goal
def shutdown(self):
rospy.loginfo(rospy.get_caller_id() + "Navigation shutting down.")
