MPC

main.cpp

//

//  main.cpp

//  MAPLESEED

//

//  Created by Michael Rivera on 3/7/24.

//

//* Adaptive MPC DWA-based Obstacles Avoidance with Turtlebot3 using ROS2 and
//Google Ceres

//* Copyright (C) MIT 2022-2023 Quang Nhat Le

//*

//* @author Quang Nhat Le - [email protected]

//* @date   2023-Sep-23 (Last updated)

//****************************************************************************/

/*

// TODO: Polish the code and add its own library "mpc_obstacle_avoid.h" to
reduce for lines for easy reading

// TODO: should change these parameters into struct config

// TODO: should make Get/Set functions to manually set goals

// TODO: path to save state and control history , configure so that everytimes
run save in different name

const std::string
fname="/home/quang_le/ros2_ws/src/hello/scripts/mpc_rover_state_obs_avoid_3.txt";
//just a directory, please change

*/

#define PRINT_CMD(x) (std::cout << x << std::endl)

#define CLAMP(x, lower, upper) \
  ((x) < (lower) ? (lower) : ((x) > (upper) ? (upper) : (x)))

using namespace std;

#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"

// #include "std_msgs/msg/float32_multi_array.hpp"

#include <ceres/ceres.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>

#include <cmath>
#include <fstream>

using ceres::AutoDiffCostFunction;

using ceres::CostFunction;

using ceres::Problem;

using ceres::Solver;

using ceres::Solve;

struct Config {
  int predictionHorizon;  // In model predictive control (MPC), both the
                          // prediction horizon and the time step are essential
                          // concepts that govern the behavior of the control
                          // algorithm.

  // Prediction Horizon: This refers to the length of time into the future over
  // which the future behavior of the system is predicted. It's essentially how
  // far ahead the controller looks when planning its control actions. A longer
  // prediction horizon allows for better anticipation of future system behavior
  // and can lead to more optimal control decisions. However, it also increases
  // computational complexity.

  double timeStep;  // This is the interval of time at which the control
                    // algorithm recomputes the control action. It represents
                    // the frequency at which the controller "updates" its
                    // decisions based on new information. A smaller time step
                    // allows for more frequent adjustments to the control
                    // signal, potentially leading to better tracking of desired
                    // setpoints and improved disturbance rejection. However, a
                    // smaller time step also increases computational demands.

  double targetX;  // THE X COORDINATE YOU WANT TO GET TO(DESIRED X POSITION)

  double targetY;  // THE Y COORDINATE YOU WANT TO GET TO (DESIRED Y POSITION)

  double lowerV;  // LOWER LIMIT FOR LINEAR VELOCITY(SPEED)

  double upperV;  // UPPER LIMIT FOR LINEAR VELOCITY(SPEED)

  double lowerW;  // LOWER LIMIT FOR ANGULAR VELOCITY(ROTATION)

  double upperW;  // UPPER LIMIT FOR ANGULAR VELOCITY(ROTATION)

  double targetV;  // DESIRED VELOCITY FROM ROBOT

  double goalWeight;  // PARAMETER USED TO DETERMINE THE "WEIGHT" OF THE GOAL
                      // POSITION( INFLUENCES HOW MUCH THE ROBOT WILL PRIORITIZE
                      // THE GOAL POSITION

  double obsWeight;  //"WEIGHT" GIVEN TO OBSTACLES . INFLUENCES HOW MUCH THE
                     //ROBOT WILL PRIORITIZE CERTAIN OBSTACLES. (IM PRETTY SURE
                     //THE HIGHER THE WEIGHT THE MORE THE ROBOT WILL TRY AND
                     //AVOID THAT OBSTACLE BUT MIGHT BE THE OTHER WAY AROUND).

  double speedWeight;  //"WEIGHT" GIVEN TO SPEED . INFLUENCES HOW MUCH THE ROBOT
                       //WILL PRIORITIZE IN MAINTAINING ITS DESIRED SPEED

  double lookaheadDist;  // PARAMETER FOR DISTANCE AHEAD OF THE ROBOT WHERE IT
                         // LOOKS TO AVOID OBSTACLES

  double safetyRadius;  // REPRESENTS THE SAFETY RADIUS AROUND THE ROBOT USED
                        // FOR OBSTACLE AVOIDANCE.IF OBSTACLE DETECTED WITHIN
                        // THIS RADIUS ROBOT WILL AVOID IT.

  double robotStuckFlagCons;  // constant to prevent robot stucked

  double minThreshold;  //

  double closeRTuned;  // for lasercall function

  double farRTuned;  // for lasercall function

  std::string fname;  // REPRESENTS THE FILE NAME AND PATH WHERE DATA COLLECTED
                      // WILL BE STORED

  // constructor

  Config()
      :

        predictionHorizon(10),

        timeStep(0.1),

        targetX(6.0),

        targetY(6.0),

        lowerV(-0.5),

        upperV(1.0),

        lowerW(-40.0 * M_PI / 180.0),

        upperW(40.0 * M_PI / 180.0),

        targetV(0.4),

        goalWeight(0.3),

        obsWeight(1.1),

        speedWeight(0.9),

        lookaheadDist(0.4),

        safetyRadius(0.3),

        robotStuckFlagCons(0.001),

        minThreshold(0.35),

        closeRTuned(0.001),

        farRTuned(100.0),

        // fileName("/home/quang_le/ros2_ws/src/hello/scripts/mpc_rover_state_obs_avoid_3.txt")
        // {};

        fname(
            "/home/ruiying/temp_wx/src/MPC_PID_ROS2_Control/"
            "mpc_rover_state_obs_avoid_3.txt")

            {};

  // no destructor needed

  // getter and setter functions

  //--------------------------------------------------------------------

  double getTargetX() const { return targetX; }

  void setTargetX(double TX) { targetX = TX; }

  double getTargetY() const { return targetY; }

  void setTargetY(double TY) { targetY = TY; }

  double getTargetV() const { return targetV; }

  void setTargetV(double TV) { targetV = TV; }

  //--------------------------------------------------------------------

  double getLowerV() const { return lowerV; }

  void setLowerV(double LV) { lowerV = LV; }

  double getUpperV() const { return upperV; }

  void setUpperV(double UV) { upperV = UV; }

  double getLowerW() const { return lowerW; }

  void setLowerW(double LW) { lowerW = LW; }

  double getUpperW() const { return upperW; }

  void setUpperW(double UW) { upperW = UW; }

  //--------------------------------------------------------------------

  double getGoalWeight() const { return goalWeight; }

  void setGoalWeight(double GW) { goalWeight = GW; }

  double getObsWeight() const { return obsWeight; }

  void setObsWeight(double OW) { obsWeight = OW; }

  double getSpeedWeight() const { return speedWeight; }

  void setSpeedWeight(double SW) { speedWeight = SW; }

  //--------------------------------------------------------------------

  int getPredictionHorizon() const { return predictionHorizon; }

  void setPredictionHorizon(int hor) { predictionHorizon = hor; }

  double getTimeStep() const { return timeStep; }

  void setTimeStep(double TS) { timeStep = TS; }

  double getLookaheadDist() const { return lookaheadDist; }

  void setLookaheadDist(double LAD) { lookaheadDist = LAD; }

  double getSafetyRadius() const { return safetyRadius; }

  void setSafetyRadius(double SR) { safetyRadius = SR; }

  //--------------------------------------------------------------------

  double getRobotStuckFlagCons() const { return robotStuckFlagCons; }

  void setRobotStuckFlagCons(double RSF) { robotStuckFlagCons = RSF; }

  double getMinThreshold() const { return minThreshold; }

  void setMinThreshold(double MT) { minThreshold = MT; }

  //--------------------------------------------------------------------

  double getCloseRTuned() const { return closeRTuned; }

  void setCloseRTuned(double CRT) { closeRTuned = CRT; }

  double getFarRTuned() const { return farRTuned; }

  void setFarRTuned(double FRT) { farRTuned = FRT; }

  std::string getFname() const { return fname; }

  void setFname(const std::string& fname) { this->fname = fname; }
};

//"

// TODO: should change these parameters into struct config

// TODO: should make Get/Set functions to manually set goals

//"

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

// one function

// ENSURES ANGLES USED IN CALCULATIONS ARE WITHIN A "CONSISTANT RANGE"

// TAKES IN A VALUE ,THEN ENTERS A WHILE LOOP THAT CONTINUES UNTIL THE ANGLE
// VALUE IS WHITHIN -PI TO PI.(-180 TO 180 DEGREES)

// this is done so that normalized angles are easier to work
// with(ex:0,360,720)and so that calculations done with angles are easier by not
// having to account for multiple representations of the same angle.

// IF VALUE IS GREATER THAN PI THAN IT SUBTRACTS 2*PI TO BRING IT BACK WITHIN
// THE DESIRED RANGE '

// IF VALUE IS LESS THAN PI ....

// ONCE THE ANGLE IS WITHIN DESIRED RANGE IT RETURNS THE NORMALIZED ANGLE VALUE

template <typename T>

T norminalAngle(T value) {
  while (*(double*)&value > M_PI || *(double*)&value < -M_PI) {
    if (*(double*)&value > M_PI)
      value -= 2.0 * T(M_PI);

    else
      value += 2.0 * T(M_PI);
  }

  return value;
}

//-------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

struct Point

{
  float x;  // x coordinate

  float y;  // y-coordinate

  float v;  // linear velocity

  float w;  // angular velocity

  float t;  // time
};

// one struct to be used later for graphing trajectory
// -------------------------------------------------------

//------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------]

// this is a functor

struct MPCProblem {
  // initial state variables:

  double x_init, y_init, yaw_init;

  // goal position you want to reach for the system

  double x_goal, y_goal;

  //                                                        x    y      x  y x y

  // a vector that stores other vector ---Example :  {   {1.3, 3.4}, {13,4} ,
  // {4.5,6.66}  }

  // the vector represents the obstacles and their coordination

  const std::vector<std::vector<double>> obs_list;

  Config config;

  // constructor:initializes the MPCProblem struct with initial state ,goal
  // position and obstacles

  MPCProblem(double x_init, double y_init, double yaw_init,

             double x_goal, double y_goal,
             const std::vector<std::vector<double>> obs_list)

      // set the initial values for MPC

      // Note: const variables must be initialized here

      : x_init(x_init),
        y_init(y_init),
        yaw_init(yaw_init),

        x_goal(x_goal),
        y_goal(y_goal),

        obs_list(obs_list) {}

  //----------------------------------------------------------------------------------------

  template <typename T>

  bool operator()(const T* const v, const T* const w, T* residual) const {
    // State variables

    T x = T(x_init);  // CURRENT X POSTION ROBOT

    T y = T(y_init);  // CURRENT Y POSITION ROBOT

    T yaw = T(yaw_init);  // CURRENT YAW ORIENTATION OF ROBOT

    // Control inputs

    T dt = T(config.getTimeStep());  // Time step

    T v_k = T(v[0]);  // linear velocity

    T w_k = T(w[0]);  // angular velocity

    // the iteration below in essence just calculates whether a obstacle is too
    // close. if it is then it assigns it a high cost , which basically means
    // avoid it . it calculates the distance to the obstacle in relation to your
    // coordinates currently.

    // MPC iterations

    for (int i = 0; i < config.getPredictionHorizon(); ++i) {
      // Update state using kinematic model

      x += v_k * cos(yaw) * dt;  // UPDATES THE CURRENT X POSTION

      y += v_k * sin(yaw) * dt;  // UPDATES THE CURRENT Y POSTION

      yaw += norminalAngle(w_k * dt);  // UPDATES CURRENT YAW ORIENTATION

      // residual means differential by the target control input

      //  goalWeight * (T(x_goal) - x) is differential for states (1/2)*x^{T}Qx

      //  speedWeight * T( ceres::abs( T(targetV) - v_k)) is differential
      //  obtained from (1/2)*u^{T}Ru

      // where Q = diag(speedWeight, speedWeight)

      // Compute residual (distance to goal)

      T x_diff = T(x_goal) -
                 x;  // CALCULATES DIFFERENCE BETWEEN THE X POSTION GOAL AND THE
                     // CURRENT X POSITION(how far robot is from goal)

      T y_diff = T(y_goal) -
                 y;  // CALCULATES DIFFERENCE BETWEEN THE Y POSTION GOAL AND THE
                     // CURRENT Y POSITION(how far is robot from goal)

      // residual[1] and residual[0] are used to help the robot minimize the
      // distance to goal. The optimizer will then adjust the control inputs to
      // reduce the errors

      residual[0] =
          config.getGoalWeight() *
          x_diff;  // weighted error from x direction to the goal (x)position

      residual[1] =
          config.getGoalWeight() *
          y_diff;  // weighted error from y direction to the goal (y) position

      // OBSTACLE AVOIDANCE

      T cost = T(0.0);  //"cost" is associated to obstacles. a high "cost"value
                        //will mean the robot is too close to the obstacle and
                        //so the optimizer will try and avoid it

      T min_dist =
          T(1e6);  // represents the minumum distance between the robot and any
                   // obstacle. its set to a high value for now.this helps
                   // assess how critical the proximity of the obstacle is

      // remember obs_list is a vector that contains other vectors

      // obs[0] represents x coordinate of obstacle

      // obs[1] represents y coordinate of obstacle

      // these two make up a vector inside the vector obs_list

      for (const auto& obs : obs_list) {
        // calculates the euclidian distance between the robots current position
        // and an obstacles position

        T dist = sqrt((x - T(obs[0])) * (x - T(obs[0])) +
                      (y - T(obs[1])) * (y - T(obs[1])));

        // updates min_dist to be the smallest distance encountoured so far
        // between robot and any obstacle

        min_dist = std::min(min_dist, dist);
      }

      // if the min_dist (which was just calculated up above) is less or equal
      // to the safety radius of the robot (which means robot is too close to
      // obstacle) then "cost" is assigned a very high value

      if (min_dist <= config.getSafetyRadius())  // not too closed

      {
        // cost = T (INFINITY);

        cost = T(1e6);

      }

      // otherwise if robot is not too close to obstacle then "cost" will be
      // inversely proprtional to the distance from obstacles(min_dist)

      else {
        cost = T(1.0 / min_dist);
      }

      residual[2] =
          config.getObsWeight() *
          cost;  // Represents the cost associated with obstacle avoidance. It
                 // ensures the robot maintains a safe distance from obstacles.
                 // If the robot is too close to an obstacle, this residual will
                 // have a high value, discouraging the optimizer from choosing
                 // paths near obstacles.

      // residual to obtain target linear velocity

      residual[3] =
          config.getSpeedWeight() * T(ceres::abs(T(config.getTargetV()) - v_k));

      // Represents the cost associated with matching the target linear
      // velocity. It encourages the robot to move at the desired speed. If the
      // robot's current velocity deviates from the target velocity, this
      // residual will increase, guiding the optimizer to adjust the velocity
      // accordingly.
    }

    return true;
  }
};

// ----------------------------------------------------------------

//----------------------------------------------------------------------------------------

//----------------------------------------------------------------------------------------

// class below is the main class that will be used for robots functionality.
// both mpcproblem struct and point struct are used here. (Config struct is more
// to set and return the values so will not be used)

// will perform obtaning obstacle positions, solve MPC problem and publish to
// robot velocities

class MPCController : public rclcpp::Node {
 public:
  MPCController(double x_goal, double y_goal);

  Config config;

  // TODO: maybe try adding a destructor for this class ? // not needed since we
  // are not allocating memory here . we use smart ptr's here which deallocate
  // memory automatically by itslef

  void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);

  // odomCallback(): Callback function called whenever odometry data is
  // received. It computes errors, solves the MPC problem, publishes control
  // inputs, and updates the trajectory.

  // Odometry is the use of data from motion sensors to estimate change in
  // position over time.

  void laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg);

  // laserCallback(): Callback function called whenever laser scan data is
  // received. It processes the laser scan data to detect obstacles.

  void printObs(const std::vector<std::vector<double>>& vec);

  // printObs(): Prints the coordinates of obstacles.

  void savePath(std::string fname, std::vector<Point>& path);

  // savePath(): Saves the robot's trajectory and saves to a file for later
  // plotting.

  void animationPlot(std::string fname);

  // animationPlot(): Plots the trajectory and states using Gnuplot.

  // void process(void);

 private:
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_pub_;

  // A SHARED POINTER THAT REFERENCES/POINTS TO THE ACTUAL PUBLISHER OBJECT

  // Note: shared pointers will handle memory management automatically

  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;

  // A SHARED POINTER THAT REFERENCES/POINTS TO THE ACTUAL SUBCRIBER OBJECT

  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;

  //----------------------------------------------------------------------------------

  // goal coordinates for x and y

  double x_goal_, y_goal_;

  // obstacle list coordinates in form of a vectors inside a single vector

  std::vector<std::vector<double>> obs_list;

  // a vector that saves the rovers trajectory in the form of a vector . inside
  // this vector has other vectors that have 5 paramters(the " point" struct)
  // example : { {x,y,v,w,t  }, {x,y,v,w,t  } , ... }    //look up above to see
  // point struct.

  std::vector<Point> Trajectory;

  // dont know what this does

  rclcpp::Time node_start_time_;

  // sensor_msgs::msg::LaserScan scan;

  // geometry_msgs::msg::Twist current_velocity;
};

//----------------------------------------------------------------------------------

//----------------------------------------------------------------------------------

//----------------------------------------------------------------------------------

//----------------------------------------------------------------------------------

// ANOTHER CONSTRUCTOR BUT NO IDEA ON HOW IT WORKS

MPCController::MPCController(double x_goal, double y_goal)
    :

      Node("mpc_obstacle_avoid"),
      x_goal_(x_goal),
      y_goal_(y_goal) {
  // THE ABOVE LINE INITIALIZES THE MPC CONTROLLER CLASS AS A ROS2 NODE WITH THE
  // NAME MPC_OBSTACLE_AVOID,THE X_GOAL AND Y_GOAL MEMBER VARIABLE ARE
  // INITIALIZED ITH THE VALUES PASSED AS ARGUMENTS REPRESENTING THE COORDINATES
  // THE ROBOT SHOULD REACH

  // Create the velocity publisher WHICH HAS THREE ARGUMENTS :

  // 1)<geometry_msgs::msg::Twist>

  // 2)cmd_vel

  // 3)10

  // THE FIRST ONE specifies the message type that will be published. In this
  // case, it's geometry_msgs::msg::Twist, which represents velocity commands.

  // HE SECOND ONE is the name of the topic to which the publisher will publish
  // messages

  // THE THIRD ONE is the QoS profile which specifies the quality of service
  // settings for the publisher. In this case, it sets the queue size to 10,
  // meaning that up to 10 messages can be stored in the publisher's queue if
  // they are published faster than they can be sent

  //(In the context of ROS 2, the queue size refers to the maximum number of
  //messages that can be stored in the publisher's outgoing message queue.

  // When a publisher publishes messages faster than the subscriber can process
  // them, the messages are stored in the queue until they can be sent to the
  // subscriber. If the queue becomes full and a new message needs to be added,
  // the oldest message in the queue is removed to make space for the new one.

  //  The queue size parameter allows you to control how many messages can be
  //  stored in the queue. Setting a larger queue size can help prevent message
  //  loss if the subscriber is temporarily unable to keep up with the rate of
  //  incoming messages. However, it also increases memory usage, as each
  //  message in the queue consumes memory.)

  velocity_pub_ =
      this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);

  // CREATES SUBCRIBER WHICH SUBCSRIBES TO messages of type:
  // nav_msgs::msg::Odometry

  //"odom" is the name of the topic to which the subscriber will subscribe. In
  //this case, it's the "odom" topic, which typically provides odometry data.

  // 10 is the queue size, specifying the maximum number of messages that can be
  // stored in the subscriber's incoming message queue. If messages are
  // published faster than they can be processed, they will be stored in this
  // queue.

  // the std::bind expression creates a function object that, when invoked, will
  // call the odomCallback member function of the MPCController class on the
  // current instance (this), passing the first argument it receives as the
  // argument to odomCallback. This function object is used as the callback
  // function for the subscription, meaning that odomCallback will be called
  // whenever a new message is received on the "odom" topic.

  // std::placeholders::_1: This is a placeholder for a function argument. In
  // this case, it represents the first argument passed to the function when it
  // is invoked. When the bound function object is invoked, the argument passed
  // to it will be used as the argument to the odomCallback function.

  odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(

      "odom", 10,

      std::bind(&MPCController::odomCallback, this, std::placeholders::_1));

  // SAME THING AS ODOM_SUB: overall,  creates a subscription and subscribes to
  // messages of type sensor_msgs::msg::LaserScan on the "scan" topic, with a
  // queue size of 10. When a new message is received on the "scan" topic, the
  // laserCallback member function of the MPCController class will be called to
  // handle the message.

  laser_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(

      "scan", 10,
      std::bind(&MPCController::laserCallback, this, std::placeholders::_1));

  // Initialize the node start time

  node_start_time_ = this->now();

  // TODO: should add IMU

  //(IMU=inertial measurement Unit)

  // process();
}

//-----------------------------------------------------------------------------------------

//-----------------------------------------------------------------------------------------

//-----------------------------------------------------------------------------------------

//-----------------------------------------------------------------------------------------

// IMPLEMENTATION OF THE ODOMCALLBACK FUNCTION

void MPCController::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
  // get current time

  rclcpp::Time current_time = this->get_clock()->now();

  // Calculate the elapsed time in seconds

  float tt = (current_time - node_start_time_).seconds();

  // collect x and y data position FROM ROBOT

  double x = msg->pose.pose.position.x;

  double y = msg->pose.pose.position.y;

  // msg->pose.pose.orientation contains the orientation of the robot
  // represented as a quaternion (geometry_msgs::msg::Quaternion).

  /// The quaternion is converted to Euler angles (roll, pitch, yaw) using the
  /// tf2::Quaternion and tf2::Matrix3x3 classes. These angles represent the
  /// rotation of the robot around the x, y, and z axes, respectively.

  // The yaw angle (theta) is extracted from the Euler angles. This angle
  // represents the heading or orientation of the robot in the horizontal plane

  const geometry_msgs::msg::Quaternion& quat = msg->pose.pose.orientation;

  // Convert the quaternion to Euler angles (roll, pitch, yaw)

  tf2::Quaternion tf_quat(quat.x, quat.y, quat.z, quat.w);

  double roll, pitch, yaw;

  // roll is the rotation around the x axis

  // pitch is the roation around the y aaxis

  // yaw is the roation around the z axis

  tf2::Matrix3x3(tf_quat).getRPY(roll, pitch, yaw);

  double theta = yaw;

  //-----------

  // Calculate the errors

  // ERROR_X AND ERROR_Y BASICALLY CALCULATES HOW FAR OR HOW CLOSE YOU ARE TO
  // THE GOAL POSTIONS(X AND Y)

  double error_x_ = x - x_goal_;

  double error_y_ = y - y_goal_;

  // this calculates the straigt line distance from where the robot is to the
  // goal position.

  double desired_dis = sqrt(pow(error_x_, 2) + pow(error_y_, 2));

  // ceres is a library which is not built in nor made by us. its a library made
  // for solving large scale optimization problems(its an open source
  // library).if you want to know what exactly the ceres functions do then look
  // up
  // "https://github.com/ceres-solver/ceres-solver/blob/master/include/ceres/ceres.h"

  // initilizes Problem object from the ceres library called "problem" and calls
  // upon different ceres functions.

  //---------------------

  ceres::Problem problem;

  ceres::CostFunction* cost_function =

      new ceres::AutoDiffCostFunction<MPCProblem, 4, 1, 1>(

          new MPCProblem(x, y, theta, x_goal_, y_goal_, obs_list));

  // TODO: create a buffer for the control inputs

  // CONTROL INPUTS ARE W AND V

  // A BUFFER(IN CONTEXT OF PROGRAMMING) IS A REGION OF MEMORY USED TO
  // TEMPORARILY HOLD DATA WHILE IT IS BEING MOVED FROM ONE PLACE TO ANOTHER OR
  // WHILE IT IS BEING PROCESSED

  // these are initialized using the previous control input

  // but more suitable initialization is required for fast optimization

  double v = 0.0;  // Linear velocity

  double w = 0.0;  // Angular velocity

  problem.AddResidualBlock(cost_function, nullptr, &v, &w);

  // set lower and upper constraints for the control inputs

  problem.SetParameterLowerBound(&v, 0, config.getLowerV());

  problem.SetParameterUpperBound(&v, 0, config.getUpperV());

  problem.SetParameterLowerBound(&w, 0, config.getLowerV());

  problem.SetParameterUpperBound(&w, 0, config.getUpperV());

  ceres::Solver::Options options;

  options.minimizer_progress_to_stdout =
      false;  //  if true, debug messages are shown

  options.linear_solver_type = ceres::DENSE_QR;

  ceres::Solver::Summary summary;

  ceres::Solve(options, &problem, &summary);

  //-------------------------

  // activates if statement, if one of the two conditions are fulfilled:

  // the absolute value of "v" ,the linear velocity, is less than
  // robotStuckFlagCons(note that this parameter will usually be close to 0 if
  // not 0),NOTE: if the velocity is less then robotStuckFlagCons it will mean
  // robot is moving very very slow or is stationary,

  //

  // OR  the linear velocity recieved from the robot itslef is less than
  // robotStuckFlagCons.

  // differenece is one chekcs from local variable while other checks from
  // actual robot data

  if (std::abs(v) < config.robotStuckFlagCons ||

      std::abs(msg->twist.twist.linear.x) < config.robotStuckFlagCons) {
    // if conditions are actived sets angular velocity to lowerbound value which
    // basically stops rotation of robot

    // to ensure the robot do not get stuck in

    // best v=0 m/s (in front of an obstacle) and

    //  best omega=0 rad/s (heading to the goal with

    // angle difference of 0)

    w = config.lowerW;

    // Note: if robot is stuck wouldnt that mean we need to set velocity to 0 as
    // well to prevent it from driving into a obsticle nonstop

    // v = lowerV;
  }

  // Publish control inputs to velocity topic

  geometry_msgs::msg::Twist velocity_msg;

  velocity_msg.linear.x = v;

  velocity_msg.angular.z = w;

  static bool isGoal = false;

  // IF THE ROBOT IS CLOSE ENOUGH TO GOAL POSTION ACTIVATES WHATS INSIDE IF
  // STATEMENT

  if (desired_dis <= config.lookaheadDist)

  {
    velocity_msg.linear.x = 0.0;

    velocity_msg.angular.z = 0.0;

    velocity_pub_->publish(velocity_msg);

    RCLCPP_INFO(get_logger(), "FINISH SUCCESS with v = %f, z = %f \n", v, w);

    v = 0.0;

    w = 0.0;

    isGoal = true;

    savePath(config.fname, Trajectory);

    animationPlot(config.fname);

    // Shutdown the ROS 2 node to terminate the program

    rclcpp::shutdown();

    return;

  }

  else {
    // Print solution

    RCLCPP_INFO(get_logger(), "MPC solution: v = %f, w = %f", v, w);
  }

  if (!isGoal)

  {
    velocity_pub_->publish(velocity_msg);
  }

  // for(const auto& obs : obs_list){

  //     double dist = sqrt((double(obs[0]) )*(double( obs[0]) ) + (double
  //     (obs[1]) )*( double(obs[1])));

  //     std::cout << "Dist = " << dist <<  " x_obs = " << obs[0] << " y_obs = "
  //     << obs[1] << std::endl;

  // }

  // saving the robot path using points

  Point p;

  p.x = (float)msg->pose.pose.position.x;  // position x

  p.y = (float)msg->pose.pose.position.y;  // position y

  p.v = (float)msg->twist.twist.linear.x;  // velocity

  p.w = (float)msg->twist.twist.angular.z;  // angular velocity

  p.t = (float)tt;  // elapsed time

  /*\


   Point p;

   p.x = static_cast<float>(msg->pose.pose.position.x); // position x

   p.y = static_cast<float>(msg->pose.pose.position.y); // position y

   p.v = static_cast<float>(msg->twist.twist.linear.x); // velocity

   p.w = static_cast<float>(msg->twist.twist.angular.z); // angular velocity

   p.t = static_cast<float>(tt); // elapsed time


   */

  // the data up above is then stored inside the trajectory vector.(as elapsed
  // tiem goes on more and more information is added to the vector)

  Trajectory.push_back(p);
}

//---------------------------------------------------------------------------------------------------

// IMPLEMENTATION OF THE LASERCALLBACK FUNCTION THAT WAS DECLARED UP ABOVE

void MPCController::laserCallback(
    const sensor_msgs::msg::LaserScan::SharedPtr msg) {
  /*

   The data being passed in sensor_msgs::msg::LaserScan::SharedPtr represents
   laser scan data. This data typically includes information collected by a
   lidar sensor, such as:

   Range measurements: Distance from the sensor to objects in its field of view.


   Intensity readings: Intensity of the reflected laser beam from objects.


   Timestamps: Time at which each measurement was taken.


   Scan angles: Angular position of each measurement relative to the sensor's
   orientation.

   */

  // list of obstacles

  // std::vector<std::vector<double>> obs_list;

  obs_list.clear();

  // double min_range = msg->range_min;  //ah for gazebo this is just 0.12m

  double max_range = msg->range_max;  // and this is 3.5m, fixed

  // i am going to guess the above line max range represenets the max range for
  // the lidar scan (max range to scan its surroundings)

  // radius

  double r;

  for (size_t i = 0; i < msg->ranges.size(); i++)

  {
    // get the radius from msg ex:

    // you have msg->ranges={2.0,3.0,1.5}

    // r=msg->ranges[[i] will give you r=2.0 for the first iteration,r=3.0 for
    // the second iteration and so on.

    double range = msg->ranges[i];

    if (config.minThreshold <= range && range <= max_range) {
      r = range;

    } else if (std::isfinite(range) && range < config.minThreshold) {
      r = config.closeRTuned;

    } else if (!std::isfinite(range) && range < 0) {
      r = 0.0;

    } else if (!std::isfinite(range) && range > 0) {
      r = (double)config.farRTuned;

    } else if (std::isnan(range)) {
      // This is an erroneous, invalid, or missing measurement.

      // Handle or ignore.

      continue;

    } else {
      // The sensor reported these measurements as valid, but they are discarded

      // per the limits defined by minimum_range and maximum_range.

      // Handle or ignore.

      continue;
    }

    //// very strong condition to force the robot not touch obstacle

    ////but trade off is it is very unstable

    // if((i >0) && (r_prev == (double)0.0) && ((r-r_prev) < thres_touch_obs)){

    //     r = 0.001;

    // }

    // calculates angle in radians(not sure what angle_min andd angle_increment
    // are)

    // This is a valid measurement.

    double angle = (double)msg->angle_min + i * msg->angle_increment;

    // for this function i am only changing the below code

    // convert the angle to degrees

    double angle_degrees = angle * 180.0 / M_PI;

    // we are going to want to filter the angles so that only angles that are
    // 60,90,and 120 are accepted(or very close to it ) .

    //"fabs" stands for floating point absolute value ,which is a function
    //provided by the standard c++ library,which just converts makes neg numbers
    //to positive and leaves positive numbers as positive

    if (fabs(angle_degrees - 60) < 1.0 ||

        fabs(angle_degrees - 90) < 1.0 ||

        fabs(angle_degrees - 120) < 1.0 ||

        fabs(angle_degrees + 60) < 1.0 ||

        fabs(angle_degrees + 90) < 1.0 ||

        fabs(angle_degrees + 120) < 1.0) {
      // Extract x and y coordinates from the scan data

      // conversion from polar coordinates to cartesian coordinates since the
      // raw extracted data from lidar comes in polar
      // coordinates((range,angle)->(x,y))

      double x = (double)r * cos(angle);

      double y = (double)r * sin(angle);

      // store the converted single x and y coordinates and store it inside the
      // vector obs_list(which stores vectors)

      std::vector<double> obs_state = {x, y};

      obs_list.push_back(obs_state);

      // TODO: maybe adding Kalman filter here ?
    }

    // printObs(obs_list);
  }
}

//-------------------------------------------------------------------------------------------

// IMPLEMTATION OF THE PRINTOBS FUNCTION THAT WAS DECLARED UP ABOVE

void MPCController::printObs(const std::vector<std::vector<double>>& vec)

{
  for (const auto& inner_vec : vec)

  {
    for (const auto& value : inner_vec)

    {
      std::cout << value << " ";
    }

    std::cout << std::endl;
  }
}

//-------------------------------------------------------------------------------------------

// IMPLEMTATION OF THE SAVEPATH FUNCTION THAT WAS DECLARED UP ABOVE

void MPCController::savePath(std::string fname, std::vector<Point>& path)

{
  std::ofstream MyFile(fname);

  for (size_t i = 0; i < path.size(); i++)

  {
    MyFile << path[i].x << " " << path[i].y << " " << path[i].v << " "
           << path[i].w << " " << path[i].t << std::endl;
  }

  MyFile.close();
}

//-------------------------------------------------------------------------------------------

// IMPLEMTATION OF THE FUNCTION ANIMATIONPLOT THAT WAS DECLARED UP ABOVE

void MPCController::animationPlot(std::string fname) {
  static FILE* gp;

  if (gp == NULL)

  {
    gp = popen("gnuplot -persist", "w");

    fprintf(gp, "set colors classic\n");

    fprintf(gp, "set grid\n");

    fprintf(gp, "set xlabel Time [s]\n");

    fprintf(gp, "set tics font \"Arial, 14\"\n");

    fprintf(gp, "set xlabel font \"Arial, 14\"\n");

    fprintf(gp, "set ylabel font \"Arial, 14\"\n");
  }

  // Plot x versus y

  fprintf(gp, "set title 'Position (x vs. y)'\n");

  fprintf(gp, "plot \"%s\" using 1:2 with lines title 'x vs. y'\n",
          fname.c_str());

  fflush(gp);

  // Wait for user input (press Enter) before plotting the next graph

  printf("Press Enter to continue...\n");

  getchar();

  // Plot v versus t

  fprintf(gp, "set title 'Linear Velocity (v vs. t)'\n");

  fprintf(gp, "plot \"%s\" using 5:3 with lines title 'v vs. t'\n",
          fname.c_str());

  fflush(gp);

  // Wait for user input (press Enter) before plotting the next graph

  printf("Press Enter to continue...\n");

  getchar();

  // Plot w versus t

  fprintf(gp, "set title 'Angular Velocity (v vs. t)'\n");

  fprintf(gp, "plot \"%s\" using 5:4 with lines title 'v vs. t'\n",
          fname.c_str());

  fflush(gp);

  // Close gnuplot

  // fclose(gp);  // Do not close gnuplot here to keep the window open

  // Uncomment the line below if you want to automatically close gnuplot

  // int retVal = system("killall -9 gnuplot\n");
}

// void MPCController::process(void){

// //TODO: something else, maybe connect with the custom Plugin Nav2 control ?

// }

//-------------------------------------------------------------------------------------------------

// EXPLANATIONS FOR DIFFERENT CONCEPTS USED IN CODE:

/*

Modularity: In ROS 2, a robot system is typically divided into small, modular
units called nodes. Each node represents a self-contained unit of functionality,
responsible for performing specific tasks such as reading sensor data,
processing information, controlling actuators, etc. This modular design promotes
code reusability, maintainability, and scalability.


Communication: Nodes communicate with each other using a publish-subscribe
messaging paradigm. They can publish messages (data) to specific topics, and
they can subscribe to topics to receive messages. This enables nodes to exchange
information, coordinate actions, and collaborate effectively within the robot
system.


Creation and Management: Nodes are created and managed using the ROS 2 API
provided by the rclcpp::Node class in C++. Nodes are typically instantiated as
instances of this class, which provides methods for creating publishers,
subscribers, timers, services, and other ROS 2 entities necessary for building a
robotic system.


Lifecycle Management: Nodes have a well-defined lifecycle, including
initialization, execution, and shutdown phases. During initialization, nodes may
perform setup tasks such as parameter configuration, resource allocation, and
establishing connections to other nodes. During execution, nodes perform their
main tasks, such as processing data and interacting with other nodes. Finally,
during shutdown, nodes clean up resources and release any acquired resources
before terminating.


Name and Namespace: Nodes have names and can exist within namespaces. Namespaces
provide a way to organize nodes into logical groups, allowing for better
management and organization of the robotic system. Each node must have a unique
name within its namespace to avoid naming conflicts.


*/

/*

 The publish-subscribe (pub-sub) messaging paradigm is a communication pattern
 used in distributed systems, including ROS (Robot Operating System), to
 facilitate message passing between different components of a system.


 -PUBLISHERS:

 Publishers are entities responsible for sending out messages (data) on specific
 topics. Each message is associated with a topic, which acts as a named channel
 for communication. Publishers create messages, populate them with data, and
 then publish (send) them to the specified topics. Multiple publishers can
 publish messages on the same topic.


 -SUBSCRIBERS:

 Subscribers are entities that receive messages published on specific topics.

 Subscribers express interest in specific topics by subscribing to them.

 When a subscriber subscribes to a topic, it indicates that it wants to
 receivE.messages published on that topic.

 Subscribers receive messages asynchronously as they are published on the
 subscribed topics.

 -TOPICS:

 Topics are named channels through which messages are exchanged between
 publishers and subscribers.

 Topics act as communication channels, allowing publishers to send messages to
 any interested subscribers.

 Messages published on a topic are typically of a specific type or format, known
 as the message type.





 IF YOU WANT TO VISUALLY UNDERSTAND YOU CAN GO TO :

 https://docs.ros.org/en/rolling/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics.html


 */

//* @brief Calling the class node object or whatever it is, not sure make_shared
//means

//*

//* @param argc

//* @param argv

//* @return int

//*/

//

int main(int argc, char** argv) {
  // A typical ROS program consists of of 4 operations :

  /*

   1)initialization

   -this is done by calling " init(...)"

   and must be done before any ROS nodes created

   2)creating one or more ROS nodes

   -creating ROS node is done by calling "create_node()" or by instantiating a
   node


   3)processesing node callback

   -after a node is created items of work can be done by spinning on the node by
   calling "spin(...)"

   4)shutdown

   -when your finished using all ROS nodes associated with the context of
   whatever your doing you call "shutdown()"


   */

  rclcpp::init(argc, argv);

  Config config_main;

  auto node =
      std::make_shared<MPCController>(config_main.targetX, config_main.targetY);

  rclcpp::spin(node);

  rclcpp::shutdown();

  return 0;
}