RTS路径规划 roborts_planner 学习笔记

学习日志 DAY 3

global_planner_node.cpp学习笔记

C++语法：

OPEN函数：

c/c++ 学习-open 函数.
open（文件打开）
open 函数用于打开和创建文件。以下是 open 函数

的简单描述:

    #include <fcntl.h>

    int open(const char *pathname, int oflag, ... /* mode_t mode */);
    返回值：成功则返回文件描述符，否则返回 -1

对于 open 函数来说，第三个参数（…）仅当创建新文件时才使用，用于指定文件的访问权限位access permission bits）。pathname 是待打开/创建文件的路径名（如 C:/cpp/a.cpp）；oflag 用于指定文件的打开/创建模式，这个参数可由以下常量（定义于fcntl.h）通过逻辑或构成。

    O_RDONLY      只读模式
    O_WRONLY      只写模式
    O_RDWR        读写模式

打开/创建文件时，至少得使用上述三个常量中的一个。以下常量是选用的：

    O_APPEND       每次写操作都写入文件的末尾
    O_CREAT        如果指定文件不存在，则创建这个文件
    O_EXCL         如果要创建的文件已存在，则返回 -1，并且修改 errno 的值
    O_TRUNC        如果文件存在，并且以只写/读写方式打开，则清空文件全部内容
    O_NOCTTY       如果路径名指向终端设备，不要把这个设备用作控制终端。
    O_NONBLOCK     如果路径名指向 FIFO/块文件/字符文件，则把文件的打开和后继 I/O
                   设置为非阻塞模式（nonblocking mode）

以下三个常量同样是选用的，它们用于同步输入输出

    O_DSYNC        等待物理 I/O 结束后再 write。在不影响读取新写入的数据的
                   前提下，不等待文件属性更新。
    O_RSYNC        read 等待所有写入同一区域的写操作完成后再进行
    O_SYNC         等待物理 I/O 结束后再 write，包括更新文件属性的 I/O

条件变量：C++11并发编程-条件变量(condition_variable)详解.
std::condition_variable是条件变量，更多有关条件变量的定义参考维基百科。Linux下使用Pthread库中的 pthread_cond_*()函数提供了与条件变量相关的功能，Windows 则参考 MSDN。

当 std::condition_variable对象的某个wait 函数被调用的时候，它使用 std::unique_lock(通过std::mutex) 来锁住当前线程。当前线程会一直被阻塞，直到另外一个线程在相同的std::condition_variable 对象上调用了 notification 函数来唤醒当前线程。

noexpect

C++11 新特性 —— 关键字noexcept.
从C++11开始，我们能看到很多代码当中都有关键字noexcept。比如下面就是std::initializer_list的默认构造函数，其中使用了noexcept。

  constexpr initializer_list() noexcept
  : _M_array(0), _M_len(0) { }

该关键字告诉编译器，函数中不会发生异常,这有利于编译器对程序做更多的优化。
如果在运行时，noexecpt函数向外抛出了异常（如果函数内部捕捉了异常并完成处理，
这种情况不算抛出异常），程序会直接终止，调用std::terminate()函数，该函数内部会调用std::abort()终止程序。

C++11条件变量：notify_one()与notify_all()的区别

C++11条件变量：notify_one()与notify_all()的区别.

notify_one()：因为只唤醒等待队列中的第一个线程；不存在
锁争用，所以能够立即获得锁。其余的线程不会被唤醒，需要等
待再次调用notify_one()或者notify_all()

notify_all()：会唤醒所有等待队列中阻塞的线程，存在锁争用，
只有一个线程能够获得锁。那其余未获取锁的线程接着会怎么样？
会阻塞？还是继续尝试获得锁？

 答案是会继续尝试获得锁(类似于轮询)，而不会再次阻塞。当持
 有锁的线程释放锁时，这些线程中的一个会获得锁。而其余的会
 接着尝试获得锁。

global_planner_node.cpp(~line 309)

 std::string full_path = ros::package::getPath("roborts_planning") + "/global_planner/config/global_planner_config.prototxt";
  if (!roborts_common::ReadProtoFromTextFile(full_path.c_str(),
                                           &global_planner_config)) {
    ROS_ERROR("Cannot load global planner protobuf configuration file.");
    return ErrorInfo(ErrorCode::GP_INITILIZATION_ERROR,
                     "Cannot load global planner protobuf configuration file.");//文件打开失败
  }


  selected_algorithm_ = global_planner_config.selected_algorithm();//配置算法
  cycle_duration_ = std::chrono::microseconds((int) (1e6 / global_planner_config.frequency()));//周期_时间
  max_retries_ = global_planner_config.max_retries();//配置最大重试次数
  goal_distance_tolerance_ = global_planner_config.goal_distance_tolerance();//配置目标距离公差
  goal_angle_tolerance_ = global_planner_config.goal_angle_tolerance();//配置目标角度公差

  // ROS path visualize
  ros::NodeHandle viz_nh("~");//节点句柄
  path_pub_ = viz_nh.advertise<nav_msgs::Path>("path", 10);//topic创建

  // Create tf listener
  tf_ptr_ = std::make_shared<tf::TransformListener>(ros::Duration(10));//坐标变换监听者创建

  // Create global costmap
  std::string map_path = ros::package::getPath("roborts_costmap") + \
      "/config/costmap_parameter_config_for_global_plan.prototxt";
  costmap_ptr_ = std::make_shared<roborts_costmap::CostmapInterface>("global_costmap",
                                                                           *tf_ptr_,
                                                                           map_path.c_str());//创建代价地图
  // Create the instance of the selected algorithm
  global_planner_ptr_ = roborts_common::AlgorithmFactory<GlobalPlannerBase,CostmapPtr >::CreateAlgorithm(
      selected_algorithm_, costmap_ptr_);//创建所选算法的实例
  if (global_planner_ptr_== nullptr) {
    ROS_ERROR("global planner algorithm instance can't be loaded");
    return ErrorInfo(ErrorCode::GP_INITILIZATION_ERROR,
                     "global planner algorithm instance can't be loaded");//无法加载全局规划器算法实例
  }


  // Initialize path frame from global costmap
  path_.header.frame_id = costmap_ptr_->GetGlobalFrameID();//从全局成本图中初始化路径框架
  return ErrorInfo(ErrorCode::OK);
}

void GlobalPlannerNode::GoalCallback(const roborts_msgs::GlobalPlannerGoal::ConstPtr &msg) {//回调规划目标（服务）

  ROS_INFO("Received a Goal from client!");

  //Update current error and info
  ErrorInfo error_info = GetErrorInfo();
  NodeState node_state = GetNodeState();

  //Set the current goal
  SetGoal(msg->goal);//设定目标

  //If the last state is not running, set it to running
  if (GetNodeState() != NodeState::RUNNING) {//将节点状态设置为运行中
    SetNodeState(NodeState::RUNNING);
  }

  //Notify the condition variable to stop lock waiting the fixed duration
  {
    std::unique_lock<std::mutex> plan_lock(plan_mutex_);
    plan_condition_.notify_one();
  }



  while (ros::ok()) {
    // Preempted and Canceled
    if (as_.isPreemptRequested()) {//抢占成功
      if (as_.isNewGoalAvailable()) {//新目标可用
        as_.setPreempted();
        ROS_INFO("Override!");//覆盖
        break;
      }else{
        as_.setPreempted();
        SetNodeState(NodeState::IDLE);
        ROS_INFO("Cancel!");//取消
        break;
      }
    }

    // Update the current state and error info更新当前状态和错误信息
    node_state = GetNodeState();
    error_info = GetErrorInfo();
    //TODO: seem useless to check state here, as it would never be IDLE state
    //TODO：在这里检查状态似乎没有用，因为它永远不会是IDLE状态
    if(node_state == NodeState::RUNNING || node_state == NodeState::SUCCESS || node_state == NodeState::FAILURE) {
      roborts_msgs::GlobalPlannerFeedback feedback;
      roborts_msgs::GlobalPlannerResult result;
      // If error occurs or planner produce new path, publish the feedback  如果发生错误或规划者产生新的路径，请公布反馈信息。
      if (!error_info.IsOK() || new_path_) {
        if (!error_info.IsOK()) {
          feedback.error_code = error_info.error_code();
          feedback.error_msg = error_info.error_msg();
          SetErrorInfo(ErrorInfo::OK());//错误处理
        }
        if (new_path_) {
          feedback.path = path_;
          new_path_ = false;
        }//更新路径
        as_.publishFeedback(feedback);
      }

      // After get the result, deal with actionlib server and jump out of the loop
      if(node_state == NodeState::SUCCESS){
        result.error_code = error_info.error_code();
        as_.setSucceeded(result,error_info.error_msg());
        SetNodeState(NodeState::IDLE);
        break;
      }//完成状态处理
      else if(node_state == NodeState::FAILURE){
        result.error_code = error_info.error_code();
        as_.setAborted(result,error_info.error_msg());
        SetNodeState(NodeState::IDLE);
        break;
      }//失败状态处理
    }
    std::this_thread::sleep_for(std::chrono::microseconds(1));//休眠，循环
  }

}

NodeState GlobalPlannerNode::GetNodeState() {
  std::lock_guard<std::mutex> node_state_lock(node_state_mtx_);
  return node_state_;
}

void GlobalPlannerNode::SetNodeState(NodeState node_state) {
  std::lock_guard<std::mutex> node_state_lock(node_state_mtx_);
  node_state_ = node_state;
}

ErrorInfo GlobalPlannerNode::GetErrorInfo() {
  std::lock_guard<std::mutex> error_info_lock(error_info_mtx_);
  return error_info_;
}

void GlobalPlannerNode::SetErrorInfo(ErrorInfo error_info) {
  std::lock_guard<std::mutex> node_state_lock(error_info_mtx_);
  error_info_ = error_info;
}

geometry_msgs::PoseStamped GlobalPlannerNode::GetGoal() {
  std::lock_guard<std::mutex> goal_lock(goal_mtx_);
  return goal_;
}

void GlobalPlannerNode::SetGoal(geometry_msgs::PoseStamped goal) {
  std::lock_guard<std::mutex> goal_lock(goal_mtx_);
  goal_ = goal;
}//更新各个参量

void GlobalPlannerNode::StartPlanning() {
  SetNodeState(NodeState::IDLE);//创建寻路节点并初始化
  plan_thread_ = std::thread(&GlobalPlannerNode::PlanThread, this);
}

void GlobalPlannerNode::StopPlanning() {
  SetNodeState(NodeState::RUNNING);//设置为运行中
  if (plan_thread_.joinable()) {
    plan_thread_.join();//阻塞主线程
  }
}

void GlobalPlannerNode::PlanThread() {//规划线程
  ROS_INFO("Plan thread start!");//寻路线程创建
  geometry_msgs::PoseStamped current_start;//当前起点
  geometry_msgs::PoseStamped current_goal;//当前目标
  std::vector<geometry_msgs::PoseStamped> current_path;//当前路径
  std::chrono::microseconds sleep_time = std::chrono::microseconds(0);//休眠时间设置
  ErrorInfo error_info;
  int retries = 0;//重试次数
  while (ros::ok()) {
    ROS_INFO("Wait to plan!");//等待规划
    std::unique_lock<std::mutex> plan_lock(plan_mutex_);
    plan_condition_.wait_for(plan_lock, sleep_time);//上锁
    while (GetNodeState()!=NodeState::RUNNING){//节点不在运行中时
      std::this_thread::sleep_for(std::chrono::microseconds(1));//阻塞进程
    }
    ROS_INFO("Go on planning!");//开始规划

    std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();//重置时间

    {
      std::unique_lock<roborts_costmap::Costmap2D::mutex_t> lock(*(costmap_ptr_->GetCostMap()->GetMutex()));
      bool error_set = false;
      //Get the robot current pose
      while (!costmap_ptr_->GetRobotPose(current_start)) {
        if (!error_set) {
          ROS_ERROR("Get Robot Pose Error.");
          SetErrorInfo(ErrorInfo(ErrorCode::GP_GET_POSE_ERROR, "Get Robot Pose Error."));
          error_set = true;
        }
        std::this_thread::sleep_for(std::chrono::microseconds(1));
      }

      //Get the robot current goal and transform to the global frame
      current_goal = GetGoal();//获取机器人的当前目标并转换到全局框架

      if (current_goal.header.frame_id != costmap_ptr_->GetGlobalFrameID()) {
        current_goal = costmap_ptr_->Pose2GlobalFrame(current_goal);//??????????????????????
        SetGoal(current_goal);//更新目标点
      }

      //Plan
      error_info = global_planner_ptr_->Plan(current_start, current_goal, current_path);

    }

    if (error_info.IsOK()) {
      //When planner succeed, reset the retry times 当规划成功时，重置重试次数
      retries = 0;
      PathVisualization(current_path);//??????????????????????

      //Set the goal to avoid the same goal from getting transformed every time
      current_goal = current_path.back();//设定目标，避免同一目标每次都被改造
      SetGoal(current_goal);

      //Decide whether robot reaches the goal according to tolerance
      if (GetDistance(current_start, current_goal) < goal_distance_tolerance_
          && GetAngle(current_start, current_goal) < goal_angle_tolerance_
          ) {
        SetNodeState(NodeState::SUCCESS);
      }//判断寻路成功
    } else if (max_retries_ > 0 && retries > max_retries_) {
      //When plan failed to max retries, return failure
      ROS_ERROR("Can not get plan with max retries( %d )", max_retries_ );
      error_info = ErrorInfo(ErrorCode::GP_MAX_RETRIES_FAILURE, "Over max retries.");
      SetNodeState(NodeState::FAILURE);
      retries=0;//判断寻路失败
    } else if (error_info == ErrorInfo(ErrorCode::GP_GOAL_INVALID_ERROR)){
      //When goal is not reachable, return failure immediately
      ROS_ERROR("Current goal is not valid!");//当目标无法达到时，立即返回失败
      SetNodeState(NodeState::FAILURE);
      retries=0;
    }
    else {
      //Increase retries
      retries++;
      ROS_ERROR("Can not get plan for once. %s", error_info.error_msg().c_str());
    }
    // Set and update the error info
    SetErrorInfo(error_info);

    // Deal with the duration to wait//处理需要等待的时间
    std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now();
    std::chrono::microseconds execution_duration =
        std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
    sleep_time = cycle_duration_ - execution_duration;

    // Report warning while planning timeout
    if (sleep_time <= std::chrono::microseconds(0)) {
      ROS_ERROR("The time planning once is %ld beyond the expected time %ld",
                execution_duration.count(),
                cycle_duration_.count());
      sleep_time = std::chrono::microseconds(0);
      SetErrorInfo(ErrorInfo(ErrorCode::GP_TIME_OUT_ERROR, "Planning once time out."));
    }
  }


  ROS_INFO("Plan thread terminated!");//规划线程终止
}

待解决问题：

ROS::ACTIONLIB

参考文章见文中链接