#ifndef _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__

#include <mkt_algorithm/diff/diff_predictive_trajectory.h>

namespace mkt_algorithm
{

  namespace diff
  {
    /**
     * @class GoStraight
     * @brief Standard Bicycle-like ScoreAlgorithm
     */
    class GoStraight : public mkt_algorithm::diff::PredictiveTrajectory
    {
    public:
      GoStraight();

      virtual ~GoStraight();

      /**
       * @brief Initialize parameters as needed
       * @param nh NodeHandle to read parameters from
       */
      virtual void initialize(
          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;

      /**
       * @brief Calculating algorithm
       * @param pose
       * @param velocity
       * @param traj
       */
      virtual mkt_msgs::Trajectory2D calculator(
          const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;

      /**
       * @brief Create a new GoStraight instance
       * @return A pointer to the new GoStraight instance
       */
      static score_algorithm::ScoreAlgorithm::Ptr create();
      
    }; // class GoStraight
  } // namespace diff

} // namespace mkt_algorithm

#endif // _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__