Need for fp_->replan_time

[ZeeshanshariffNS]

"Hello @ZbyLGsc , thank you for your code contribution. I noticed that in the PLAN_TRAJ state, you are calculating t_r by adding fp_->replan_time_ to the current time. Could you please explain the reasoning behind this? I am interested in understanding this part of the code better. Thank you!"

case PLAN_TRAJ: {
  if (fd_->static_state_) {
    // Plan from static state (hover)
    fd_->start_pt_ = fd_->odom_pos_;
    fd_->start_vel_ = fd_->odom_vel_;
    fd_->start_acc_.setZero();

    fd_->start_yaw_(0) = fd_->odom_yaw_;
    fd_->start_yaw_(1) = fd_->start_yaw_(2) = 0.0;
  } 
  else {
    // Replan from non-static state, starting from 'replan_time' seconds later
    LocalTrajData* info = &planner_manager_->local_data_;
    double t_r = (ros::Time::now() - info->start_time_).toSec() + fp_->replan_time_;

    fd_->start_pt_ = info->position_traj_.evaluateDeBoorT(t_r);
    fd_->start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_r);
    fd_->start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_r);
    fd_->start_yaw_(0) = info->yaw_traj_.evaluateDeBoorT(t_r)[0];
    fd_->start_yaw_(1) = info->yawdot_traj_.evaluateDeBoorT(t_r)[0];
    fd_->start_yaw_(2) = info->yawdotdot_traj_.evaluateDeBoorT(t_r)[0];

    float eucledian_dist = sqrt(pow(fd_->start_pt_(0) - fd_->odom_pos_(0), 2) +
                        pow(fd_->start_pt_(1) - fd_->odom_pos_(1), 2) +
                        pow(fd_->start_pt_(2) - fd_->odom_pos_(2), 2) * 1.0);
    ROS_ERROR("Tracking error:  %f",eucledian_dist);
    cout<<"eucledian_dist:" <<eucledian_dist<<std::endl;

  }

  if (fd_->no_frontier_flag){
    transitState(FINISH, "FSM");
    break;
  }
  // Inform traj_server the replanning
  replan_pub_.publish(std_msgs::Empty());
  int res = callExplorationPlanner();
  if (res == SUCCEED) {
    transitState(PUB_TRAJ, "FSM");
  } 
  else if (res == NO_FRONTIER) {
    fd_->no_frontier_flag = true;
    transitState(FINISH, "FSM");
    fd_->static_state_ = true;
    clearVisMarker();
    break;

  } 
  else if (res == FAIL) {
    // Still in PLAN_TRAJ state, keep replanning
    ROS_WARN("plan fail");
    fd_->static_state_ = true;
  }
  break;
}