class RobotControl(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 AprilTagLocalization object: broadcasts the static pose transforms # and calculates the base_link pose in map frame from on Apriltag detections self._apriltag_localization = AprilTagLocalization(tag_poses, poselist_base2cam) 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=10) self._br = tf.TransformBroadcaster() def process_measurements(self, goal): """ YOUR CODE HERE Main loop of the robot - where all measurements, control, and estimation are done. """ self._apriltag_localization.broadcast_static_tf() odom_meas = self._base_controller.get_measurement() # current wheel angles (wheel_angle_left, wheel_angle_right) tag_meas = self._apriltag_localization.get_measurement() # calculated robot pose(s) based on Apriltag detections (x, y, theta) # 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.") self._base_controller.shutdown()