def __init__(self):
        # Initialize ROS node
        rospy.init_node('turtlebot_pose_controller', anonymous=True)
        self.params = PoseControllerParams(verbose=True)

        # Current state
        self.x = 0.
        self.y = 0.
        self.theta = 0.

        # Controller from HW1 P2
        self.controller = PoseController(self.params.k1, self.params.k2,
                                         self.params.k3, self.params.v_max,
                                         self.params.om_max)

        self.controller.load_goal(self.x, self.y, self.theta)

        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        # Time last pose command was received
        self.cmd_pose_time = rospy.get_rostime()

        # If using gazebo, we have access to perfect state
        if self.params.use_gazebo:
            rospy.Subscriber('/gazebo/model_states', ModelStates,
                             self.gazebo_callback)
        self.trans_listener = tf.TransformListener()

        ########## Code starts here ##########
        # TODO: Create a subscriber to the '/cmd_pose' topic that receives
        #       Pose2D messages and calls cmd_pose_callback.
        rospy.Subscriber('/cmd_pose', Pose2D, self.cmd_pose_callback)
class PoseControllerNode:
    def __init__(self):
        # Initialize ROS node
        rospy.init_node('turtlebot_pose_controller', anonymous=True)
        self.params = PoseControllerParams(verbose=True)

        # Current state
        self.x = 0.
        self.y = 0.
        self.theta = 0.

        # Controller from HW1 P2
        self.controller = PoseController(self.params.k1, self.params.k2,
                                         self.params.k3, self.params.v_max,
                                         self.params.om_max)

        self.controller.load_goal(self.x, self.y, self.theta)

        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        # Time last pose command was received
        self.cmd_pose_time = rospy.get_rostime()

        # If using gazebo, we have access to perfect state
        if self.params.use_gazebo:
            rospy.Subscriber('/gazebo/model_states', ModelStates,
                             self.gazebo_callback)
        self.trans_listener = tf.TransformListener()

        ########## Code starts here ##########
        # TODO: Create a subscriber to the '/cmd_pose' topic that receives
        #       Pose2D messages and calls cmd_pose_callback.
        rospy.Subscriber('/cmd_pose', Pose2D, self.cmd_pose_callback)
        ########## Code ends here ##########

    def gazebo_callback(self, msg):
        if "turtlebot3_burger" not in msg.name:
            return

        pose = msg.pose[msg.name.index("turtlebot3_burger")]
        self.x = pose.position.x
        self.y = pose.position.y
        quaternion = (pose.orientation.x, pose.orientation.y,
                      pose.orientation.z, pose.orientation.w)
        euler = tf.transformations.euler_from_quaternion(quaternion)
        self.theta = euler[2]

    def cmd_pose_callback(self, msg):
        ########## Code starts here ##########
        # TODO: Update the goal pose in the pose controller.
        self.controller.load_goal(msg.x, msg.y, msg.theta)
        ########## Code ends here ##########

        # Record time of pose update
        self.cmd_pose_time = rospy.get_rostime()

    def compute_control(self):
        if rospy.get_rostime().to_sec() - self.cmd_pose_time.to_sec(
        ) > self.params.timeout:
            # haven't received a command in a while so stop
            rospy.loginfo("Pose controller TIMEOUT: commanding zero controls")
            cmd = Twist()
            cmd.linear.x = 0.
            cmd.angular.z = 0.
            return cmd

        # if you are not using gazebo, your need to use a TF look-up to find robot's states
        # relevant for hw 3+
        if not self.params.use_gazebo:
            try:
                origin_frame = "/map" if self.params.mapping else "/odom"
                translation, rotation = self.trans_listener.lookupTransform(
                    origin_frame, '/base_footprint', rospy.Time(0))
                self.x, self.y = translation[0], translation[1]
                self.theta = tf.transformations.euler_from_quaternion(
                    rotation)[2]
            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException):
                pass

        ######### YOUR CODE HERE ############
        # TODO: Use your pose controller to compute controls (V, om) given the
        #       robot's current state.
        V, om = self.controller.compute_control(self.x, self.y, self.theta, 0)
        ######### END OF YOUR CODE ##########

        cmd = Twist()
        cmd.linear.x = V
        cmd.angular.z = om
        return cmd

    def run(self):
        rate = rospy.Rate(10)  # 10 Hz
        while not rospy.is_shutdown():
            ctrl_output = self.compute_control()
            self.pub.publish(ctrl_output)
            rate.sleep()