custom_controller.cpp

geometry_msgs::msg::TwistStamped CustomController::computeVelocityCommands(const geometry_msgs::msg::PoseStamped & pose,
  const geometry_msgs::msg::Twist &  , nav2_core::GoalChecker * ) 
{

/*

  static auto start_time_MPC = std::chrono::high_resolution_clock::now();

    // Convert to system_clock time point (for human-readable output)
  //  auto start_time_s = std::chrono::system_clock::to_time_t(start_time_MPC);
    
    // Print the end time in a human-readable format
    //std::cout << "Start time: " << std::ctime(&start_time_s) << std::endl;



  static auto start_time_lin = std::chrono::high_resolution_clock::now();

  double v_act,w_act;


    // Convert to system_clock time point (for human-readable output)
   // auto start_time_s2 = std::chrono::system_clock::to_time_t(start_time_lin);
    
    // Print the end time in a human-readable format
   // std::cout << "Start time lin: " << std::ctime(&start_time_s2) << std::endl;





  // Convert pose.pose.orientation from Quaternion to Roll,Pitch,Yaw
  double roll, pitch, yaw;
  tf2::Quaternion quat;
  tf2::fromMsg(pose.pose.orientation, quat);
  tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

  //std::cout<< "yaw =  "<<yaw<<std::endl;


  MPC_->set_actualRobotState(Eigen::Vector3d(pose.pose.position.x,pose.pose.position.y, yaw));

 // Find the closest pose on the path to the robot
  auto transformation_begin =
      min_by(
          global_plan_.poses.begin(), global_plan_.poses.end(),
          //&pose is current robot pose, ps is the other pose to go in euclidean_distance
          [&pose](const geometry_msgs::msg::PoseStamped &ps)
          {
            return euclidean_distance(pose, ps);
          });

  // From the closest point, look for the first point that's 0.4m away
  auto transformation_end = std::find_if(
      transformation_begin, end(global_plan_.poses),
      [&](const auto &global_plan_pose)
      {
        return euclidean_distance(pose, global_plan_pose) > 0.4; 
      });

  // assign each consecutive goal pose 0.4m away from the previous
  auto target_pose_ = *transformation_end;

// Convert pose.pose.orientation from Quaternion to Roll,Pitch,Yaw
  double roll2, pitch2, yaw2;
  tf2::Quaternion quat2;
  tf2::fromMsg(target_pose_.pose.orientation, quat2);
  tf2::Matrix3x3(quat2).getRPY(roll2, pitch2, yaw2);

//  std::cout<< "yaw2 =  "<<yaw2<<std::endl;



//MPC_->set_referenceRobotState(Eigen::Vector3d(target_pose_.pose.position.x, target_pose_.pose.position.y,45));

MPC_->set_referenceRobotState(Eigen::Vector3d(1.0, 0.0,0));



if (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start_time_MPC).count() >= 200) // execute the MPC every 0.2 seconds
 {

    // Get the current time point
    //auto end_time = std::chrono::high_resolution_clock::now();
    
    // Convert to system_clock time point (for human-readable output)
   // auto end_time_s = std::chrono::system_clock::to_time_t(end_time);
    
    // Print the end time in a human-readable format
  //  std::cout << "End time: " << std::ctime(&end_time_s) << std::endl;

  
 // std::cout<<"MPC executed after 3 seconds"<<std::endl;

  double vPx_act, vPy_act;

  // compute MPC control and optimisation to obtain optimal control inputs 
  // xp dot and yp dot to be used by the feedback linearisation to get v and w

  MPC_->executeMPCcontroller();

  Eigen::VectorXd MPC_actControl;

  // get xp dot and yp dot that are computed by the MPC
  MPC_->get_actualMPCControl(MPC_actControl);

  vPx_act = MPC_actControl(0);
  vPy_act = MPC_actControl(1);

  start_time_MPC = std::chrono::high_resolution_clock::now();
 }

if (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start_time_lin).count() >= 10) // execute the FBLIN every 0.01 seconds
{

 // std::cout<<"Linearization executed after 0.01 seconds"<<std::endl;


    // Get the current time point
   // auto end_time = std::chrono::high_resolution_clock::now();
    
    // Convert to system_clock time point (for human-readable output)
   // auto end_time_s = std::chrono::system_clock::to_time_t(end_time);
    
    // Print the end time in a human-readable format
  //  std::cout << "End time linearization: " << std::ctime(&end_time_s) << std::endl;

  MPC_->executeLinearizationController();

  start_time_lin = std::chrono::high_resolution_clock::now();


}

// Get actual control signal
// store computed v and w from fblin class to v_act, w_act variables
 MPC_->get_actualControl(v_act, w_act);





  // Apply feedback linearization
  cmd_vel_.linear.x = v_act;
  cmd_vel_.angular.z = w_act;


      cmd_vel_pub_->publish(cmd_vel_);

  //cmd_vel_.header.frame_id = pose.header.frame_id;
  //cmd_vel_.header.stamp = clock_->now();



// std::cout<<"cmd_vel lin speed"<<cmd_vel_.twist.linear.x<<std::endl;
//std::cout<<"cmd vel ang speed"<<cmd_vel_.twist.angular.z<<std::endl;
  




*/


//////// Feedback linearization only







/*

  double epsilon_ = 0.15;

  // Convert pose.pose.orientation from Quaternion to Roll,Pitch,Yaw
  double roll, pitch, yaw;
  tf2::Quaternion quat;
  tf2::fromMsg(pose.pose.orientation, quat);
  tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

  // Record the start time
  auto start_time = std::chrono::high_resolution_clock::now();

  // centroid.obstacles.begin(),centroid.obstacles.end()


  // Find the closest pose on the path to the robot
  auto transformation_begin =
      min_by(
          global_plan_.poses.begin(), global_plan_.poses.end(),
          //&pose is current robot pose, ps is the other pose to go in euclidean_distance
          [&pose](const geometry_msgs::msg::PoseStamped &ps)
          {
            return euclidean_distance(pose, ps);
          });

  // From the closest point, look for the first point that's 0.4m away
  auto transformation_end = std::find_if(
      transformation_begin, end(global_plan_.poses),
      [&](const auto &global_plan_pose)
      {
        return euclidean_distance(pose, global_plan_pose) > 0.4; 
      });

  // assign each consecutive goal pose 0.4m away from the previous
  auto target_pose_ = *transformation_end;

  // Record the end time
  auto end_time = std::chrono::high_resolution_clock::now();

  // Calculate the duration
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);

  // Print the execution time
 // std::cout << "Execution time: " << duration.count() << " microseconds" << std::endl;

  // calculate point P at a distance epsilon from the robot
  feedback_lin_.calcPointP(pose, yaw, epsilon_);

  // Apply proportional control for trajectory tracking without feed forward term
  double xp_dot_ = (target_pose_.pose.position.x - feedback_lin_.getPointP()[0]) * 2.5;
  double yp_dot_ = (target_pose_.pose.position.y - feedback_lin_.getPointP()[1]) * 1.5;
 
  // Apply feedback linearization
  cmd_vel_ = feedback_lin_.linearize(xp_dot_, yp_dot_);

  cmd_vel_.header.frame_id = pose.header.frame_id;
  cmd_vel_.header.stamp = clock_->now();

 // std::cout<<"cmd_vel lin speed"<<cmd_vel_.twist.linear.x<<std::endl;
 // std::cout<<"cmd vel ang speed"<<cmd_vel_.twist.angular.z<<std::endl;

*/



MPC();

 // return cmd_vel_;
cmd_vel.twist.linear.x = cmd_vel_.linear.x;
cmd_vel.twist.angular.z = cmd_vel_.angular.z;

 return cmd_vel;
}


void CustomController::setPlan(const nav_msgs::msg::Path &path )
{

  global_plan_ = path;

}

void CustomController::MPC()
{


  double v_act,w_act;
  auto start = std::chrono::high_resolution_clock::now();
    auto end = start + std::chrono::seconds(30); // End time

    auto start_MPC_timer = std::chrono::high_resolution_clock::now(); // Start timer for 0.2-second interval
    auto start_fblin_timer = std::chrono::high_resolution_clock::now(); // Start timer for 0.2-second interval


    while (std::chrono::high_resolution_clock::now() < end)
    {
      auto start_time_MPC = std::chrono::high_resolution_clock::now();

        // Convert pose.pose.orientation from Quaternion to Roll,Pitch,Yaw
      double roll, pitch, yaw;
      tf2::Quaternion quat;
      tf2::fromMsg(robot_pose_.pose.orientation, quat);
      tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

      MPC_->set_actualRobotState(Eigen::Vector3d(robot_pose_.pose.position.x, robot_pose_.pose.position.y, yaw));

      MPC_->set_referenceRobotState(Eigen::Vector3d(1, 0.0, 0));

        // Check if 0.2 seconds have passed since the last MPC execution
      if (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start_MPC_timer).count() >= 200)
      {
           // std::cout << "MPC executed after 2 seconds" << std::endl;


       double vPx_act, vPy_act;

  // compute MPC control and optimisation to obtain optimal control inputs 
  // xp dot and yp dot to be used by the feedback linearisation to get v and w

       MPC_->executeMPCcontroller();

       Eigen::VectorXd MPC_actControl;

  // get xp dot and yp dot that are computed by the MPC
       MPC_->get_actualMPCControl(MPC_actControl);

       vPx_act = MPC_actControl(0);
       vPy_act = MPC_actControl(1);


        // Reset the timer for the next 0.2-second interval
       start_MPC_timer = std::chrono::high_resolution_clock::now();
     }


     if (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start_fblin_timer).count() >= 10)
     {

       MPC_->executeLinearizationController();


       start_fblin_timer = std::chrono::high_resolution_clock::now();
     }

      // Get actual control signal
// store computed v and w from fblin class to v_act, w_act variables
     MPC_->get_actualControl(v_act, w_act);



    // std::cout<<"cmd_vel lin speed"<<v_act<<std::endl;
    // std::cout<<"cmd vel ang speed"<<w_act<<std::endl;




  // Apply feedback linearization
     cmd_vel_.linear.x = v_act;
     cmd_vel_.angular.z = w_act;

     // cmd_vel_.header.frame_id = robot_pose_.header.frame_id;
      //cmd_vel_.header.stamp = clock_->now();

      cmd_vel_pub_->publish(cmd_vel_);


     
   }
 }

void CustomController::setSpeedLimit(const double & , const bool & )
{

}

} // namespace

// Register this controller as a nav2_core plugin
PLUGINLIB_EXPORT_CLASS(nav2_custom_controller::CustomController, nav2_core::Controller)