How to create waypoints for pure pursuit controller

edit

I am tring to create a pure pursuit algorithm to control a tricycle vehicle on the gazebo based on the waypoints i created. I set the vehicle move as i start the simulation but i need a path to follow , for this reason i write a global path planner as plugin so i created two header files and one main file. i just copied the codes on this tutorial navigationTutorialsWriting A Global Path Planner As Plugin in ROS and pasted it, now i don't know how to do rest of it.

#include "pure_pursuit/pure_pursuit.h"
#include <pluginlib/class_list_macros.h>
#include "pure_pursuit/global_planner.h"


vec_control::PurePursuit::PurePursuit() {
  ros::NodeHandle nh_;
  ros::NodeHandle nh_private("~");
  // Node parameters
  nh_private.param<double>("ld_gain", ld_gain_, 1.0);
  nh_private.param<double>("min_ld", min_ld_, 0.5);
  nh_private.param<double>("car_wheel_base", car_wheel_base_, 0.44);
  nh_private.param<int>("controller_freq", controller_freq_, 10);
  nh_private.param<std::string>("map_frame", map_frame_, "map");
  nh_private.param<std::string>("base_frame", base_frame_, "base_link");
  ld_ = min_ld_;
  ros_rate_ = new ros::Rate(controller_freq_);
  // Publishers and subscribers
  control_pub_ = nh_.advertise<geometry_msgs::Twist>(
      "/tricycle_controller/cmd_vel", 1);

  ros::Subscriber odom_sub_ =
      nh_.subscribe("/tricycle_controller/odom", 1,
       &PurePursuit::odom_clk_, this);

  ros::Subscriber path_sub_ =
      nh_.subscribe("/move_base/DWAPlannerROS/local_plan", 1, 
      &PurePursuit::path_clk_, this);

  tfListener_ = new tf2_ros::TransformListener(tfBuffer_);
  l_point_pub_ = nh_.advertise<geometry_msgs::PointStamped>(
      "/pure_pursuit/lookahead_point", 1);

  // main loop
  control_loop_();
}

void vec_control::PurePursuit::odom_clk_(
    const nav_msgs::Odometry::ConstPtr &msg) {
  car_speed_ = msg->twist.twist.linear.x;
  ld_ = std::max(ld_gain_ * car_speed_, min_ld_);
}
//target path to follow , topic_name = /pure_pursuit/path
//type nav_msgs/Path
void vec_control::PurePursuit::path_clk_(const nav_msgs::Path::ConstPtr &msg) {
  ROS_INFO("New path is received.");
  path_ = msg->poses;
  // path_.push_back(msg->poses[0]);
  got_path_ = true;
  path_done_ = false;
  point_idx_ = 0;
  double start_end_dist =
  distance(path_[0].pose.position, path_.back().pose.position);
  ROS_INFO("Start to End Distance: %f", start_end_dist);
  ROS_INFO("Min lookup distance: %f", min_ld_);
  if (start_end_dist <= min_ld_) {
    loop_ = true;
    ROS_INFO("Is Loop: True");
  }
}
//The main loop that will run and control the vehicle on the map.
void vec_control::PurePursuit::control_loop_() {
  double y_t = 0, ld_2 = 0, delta = 0;
  double distance_ = 0;
  while (ros::ok()) {
    if (got_path_) {//if you got the path
      // get the current robot location by tf base_link -> map
      // iterate over the path points
      // if the distance between a point and robot >  lookahead then break and take
      // this point transform this point to the robot base_link the y component
      // of this point is y_t delta can be computed as atan2(2 * yt * L_, ld_2)
      try {
        base_location_ = tfBuffer_.lookupTransform(
            map_frame_, base_frame_, ros::Time(0), ros::Duration(0.1));

        for (; point_idx_ < path_.size(); point_idx_++) {
          distance_ = distance(path_[point_idx_].pose.position,
                               base_location_.transform.translation);
          ROS_INFO("Point ID: %d, Distance %f", point_idx_, distance_);
          if (distance_ >= ld_) {
            path_[point_idx_].header.stamp =
                ros::Time::now(); // Set the timestamp to now for the transform
                                  // to work, because it tries to transform the
                                  // point at the time stamp of the input point
            tfBuffer_.transform(path_[point_idx_], target_point_, base_frame_,
                                ros::Duration(0.1));
            break;
          }
        }

        ld_2 = ld_ * ld_;
        y_t = target_point_.pose.position.y;
        delta = atan2(2 * car_wheel_base_ * y_t, ld_2);

        /* control_msg_.drive.steering_angle = delta;
        control_msg_.drive.speed = 2;
        control_msg_.header.stamp = ros::Time::now(); */
        control_msg_ ...