探索算法：dsvp学习笔记（exploration.cpp）

前言：笔者研0学习中，理解有限，也在学习该算法的朋友可以共同交流学习。

1、首先从main函数开始，节点初始化和获取参数服务器数据并初始化了发布者和订阅者

ros::init(argc, argv, "exploration");
  ros::NodeHandle nh;
  ros::NodeHandle nhPrivate = ros::NodeHandle("~");
  
  nhPrivate.getParam("simulation", simulation); //"false"
  nhPrivate.getParam("/interface/dtime", dtime); //"1"
  nhPrivate.getParam("/interface/initX", init_x);//"4"
  nhPrivate.getParam("/interface/initY", init_y);//"0"
  nhPrivate.getParam("/interface/initZ", init_z);//"0"
  nhPrivate.getParam("/interface/initTime", init_time);//"4"
  nhPrivate.getParam("/interface/returnHomeThres", return_home_threshold);//"1.5"
  nhPrivate.getParam("/interface/robotMovingThres", robot_moving_threshold);//"6.0"
  nhPrivate.getParam("/interface/tfFrame", map_frame);//"map"
  nhPrivate.getParam("/interface/autoExp", begin_signal);//"true"
  nhPrivate.getParam("/interface/waypointTopic", waypoint_topic);//"/way_point"
  nhPrivate.getParam("/interface/cmdVelTopic", cmd_vel_topic);//"/cmd_vel"
  nhPrivate.getParam("/interface/graphPlannerCommandTopic", gp_command_topic);//"/graph_planner_command"
  nhPrivate.getParam("/interface/effectivePlanTimeTopic", effective_plan_time_topic);//"/runtime"
  nhPrivate.getParam("/interface/totalPlanTimeTopic", total_plan_time_topic);//"/totaltime"
  nhPrivate.getParam("/interface/gpStatusTopic", gp_status_topic);//"/graph_planner_status"
  nhPrivate.getParam("/interface/odomTopic", odom_topic);//"/state_estimation"
  nhPrivate.getParam("/interface/beginSignalTopic", begin_signal_topic);//"/start_exploring"
  nhPrivate.getParam("/interface/stopSignalTopic", stop_signal_topic);//"stop_exploring"

  //init publisher
  waypoint_pub = nh.advertise<geometry_msgs::PointStamped>(waypoint_topic, 5);//"way_point"
  gp_command_pub = nh.advertise<graph_planner::GraphPlannerCommand>(gp_command_topic, 1);//"/graph_planner_command"
  effective_plan_time_pub = nh.advertise<std_msgs::Float32>(effective_plan_time_topic, 1);//"/runtime"
  total_plan_time_pub = nh.advertise<std_msgs::Float32>(total_plan_time_topic, 1);//"/totaltime"

  //init subscribe
  gp_status_sub = nh.subscribe<graph_planner::GraphPlannerStatus>(gp_status_topic, 1, gp_status_callback);//"//"/graph_planner_status""
  waypoint_sub = nh.subscribe<geometry_msgs::PointStamped>(waypoint_topic, 1, waypoint_callback);//"/way_point"
  odom_sub = nh.subscribe<nav_msgs::Odometry>(odom_topic, 1, odom_callback);//"/state_estimation"
  begin_signal_sub = nh.subscribe<std_msgs::Bool>(begin_signal_topic, 1, begin_signal_callback);//"/start_exploring"
  stop_signal_pub = nh.advertise<std_msgs::Bool>(stop_signal_topic, 1);//"stop_exploring"

其中/way_point话题在后续过程中会自发自接。

2、等待begin_signal值为1

  ros::Duration(1.0).sleep();
  ros::spinOnce();
  while (!begin_signal)
  {
    ros::Duration(0.5).sleep();
    ros::spinOnce();
    ROS_INFO("Waiting for Odometry");
  }

3、planner成功启动，进行初始化

  ROS_INFO("Starting the planner: Performing initialization motion");
  initilization();
  ros::Duration(1.0).sleep();

4、initilization函数接收坐标为(4,0,0)的点，并将其转化到/map坐标系下，最终确保机器人能够移动到该点。

/*
  初始化函数
  首先强制发布一个目标点（4，0，0）前进
  再将出发时的点设置为返回点
  然后我们要保证小车往前走了一段距离即满足dist < 0.5 && dist_to_home > 0.5
  或者满足init_time_count >= init_time / 0.1 && dist_to_home > 0.5
  此时跳出该函数，程序进入正常探索模式
*/
void initilization()
{
  tf::Vector3 vec_init(init_x, init_y, init_z);
  tf::Vector3 vec_goal;
  vec_goal = transformToMap * vec_init;//坐标变化，把vec_init的坐标转化到transformToMap的坐标下，即map坐标
  geometry_msgs::PointStamped wp;
  wp.header.frame_id = map_frame;//"/map"
  wp.header.stamp = ros::Time::now();
  wp.point.x = vec_goal.x();
  wp.point.y = vec_goal.y();
  wp.point.z = vec_goal.z();
  home_point.x = current_odom_x;
  home_point.y = current_odom_y;
  home_point.z = current_odom_z;

  ros::Duration(0.5).sleep();  // wait for sometime to make sure waypoint can be
                               // published properly

  waypoint_pub.publish(wp);
  bool wp_ongoing = true;
  int init_time_count = 0;//初始化计时
  while (wp_ongoing)
  {  // Keep publishing initial waypoint until the robot
    // reaches that point
    init_time_count++;
    ros::Duration(0.1).sleep();
    ros::spinOnce();
    vec_goal = transformToMap * vec_init;
    wp.point.x = vec_goal.x();
    wp.point.y = vec_goal.y();
    wp.point.z = vec_goal.z();
    waypoint_pub.publish(wp);
    double dist = sqrt((wp.point.x - current_odom_x) * (wp.point.x - current_odom_x) +
                       (wp.point.y - current_odom_y) * (wp.point.y - current_odom_y));
    double dist_to_home = sqrt((home_point.x - current_odom_x) * (home_point.x - current_odom_x) +
                               (home_point.y - current_odom_y) * (home_point.y - current_odom_y));
    if (dist < 0.5 && dist_to_home > 0.5)
      wp_ongoing = false;
    if (init_time_count >= init_time / 0.1 && dist_to_home > 0.5)
      wp_ongoing = false;
  }
}

其中vec_goal = transformToMap * vec_init;这句困扰我很久，一直不知道这个重载在算什么。网上找了很久，直到看到该博主的文章。

其实就是将vec_init的坐标转化到/map坐标系下，因为vec_init的坐标是基于odom坐标系的。

5、初始化完成，进入核心逻辑

  std::cout << std::endl << "\033[1;32mExploration Started\033[0m\n" << std::endl;
  total_time.data = 0;
  plan_start = steady_clock::now();
  // Start planning: The planner is called and the computed goal point sent to
  // the graph planner.
  int iteration = 0;
  while (ros::ok())
  {
    if (!return_home)
    {
      if (iteration != 0)
      {
        for (int i = 0; i < 8; i++)
        {
          printf(cursup);
          printf(cursclean);
        }
      }
      std::cout << "Planning iteration " << iteration << std::endl;
      dsvplanner::dsvplanner_srv planSrv;
      dsvplanner::clean_frontier_srv cleanSrv;
      planSrv.request.header.stamp = ros::Time::now();
      planSrv.request.header.seq = iteration;
      planSrv.request.header.frame_id = map_frame;//"/map"
      if (ros::service::call("drrtPlannerSrv", planSrv))
      {
        if (planSrv.response.goal.size() == 0)
        {  // usually the size should be 1 if planning successfully
          ros::Duration(1.0).sleep();
          continue;
        }

        if (planSrv.response.mode.data == 2)
        {
          return_home = true;
          goal_point = home_point;
          std::cout << std::endl << "\033[1;32mExploration completed, returning home\033[0m" << std::endl << std::endl;
          effective_time.data = 0;
          effective_plan_time_pub.publish(effective_time);
        }
        else
        {
          return_home = false;
          goal_point = planSrv.response.goal[0];
          plan_over = steady_clock::now();
          time_span = plan_over - plan_start;
          effective_time.data = float(time_span.count()) * steady_clock::period::num / steady_clock::period::den;
          effective_plan_time_pub.publish(effective_time);
        }
        total_time.data += effective_time.data;
        total_plan_time_pub.publish(total_time);

        if (!simulation)
        {  // when not in simulation mode, the robot will go to
           // the goal point according to graph planner
          graph_planner_command.command = graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION;
          graph_planner_command.location = goal_point;
          gp_command_pub.publish(graph_planner_command);//"/graph_planner_command" 通过这个话题把目标点发出去
          ros::Duration(dtime).sleep();  // give sometime to graph planner for
                                         // searching path to goal point
          ros::spinOnce();               // update gp_in_progree 让spinOnce去回调函数队列调用回调函数，直到gp_in_progress=true
          int count = 200;
          previous_odom_x = current_odom_x;
          previous_odom_y = current_odom_y;
          previous_odom_z = current_odom_z;
          while (gp_in_progress)
          {                            
                                         // if the waypoint keep the same for 20
                                         // (200*0.1)
            ros::Duration(0.1).sleep();  // seconds, then give up the goal
            wayPoint_pre = wayPoint;
            ros::spinOnce();
            bool robotMoving = robotPositionChange();
            if (robotMoving)
            {

              count = 200;
            }
            else
            {
              count--;
            }
            if (count <= 0)
            {  // when the goal point cannot be reached, clean
               // its correspoinding frontier if there is
              cleanSrv.request.header.stamp = ros::Time::now();
              cleanSrv.request.header.frame_id = map_frame;
              ros::service::call("cleanFrontierSrv", cleanSrv);
              ros::Duration(0.1).sleep();
              break;
            }
          }

          graph_planner_command.command = graph_planner::GraphPlannerCommand::COMMAND_DISABLE;
          gp_command_pub.publish(graph_planner_command);
        }
        else
        {  // simulation mode is used when testing this planning algorithm
           // with bagfiles where robot will
          // not move to the planned goal. When in simulation mode, robot will
          // keep replanning every two seconds
          for (size_t i = 0; i < planSrv.response.goal.size(); i++)
          {
            graph_planner_command.command = graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION;
            graph_planner_command.location = planSrv.response.goal[i];
            gp_command_pub.publish(graph_planner_command);
            ros::Duration(2).sleep();
            break;
          }
        }
        plan_start = steady_clock::now();
      }
      else
      {
        std::cout << "Cannot call drrt planner." << std::flush;

        ros::Duration(1.0).sleep();
      }
      iteration++;
    }
    else
    {
      ros::spinOnce();
      if (fabs(current_odom_x - home_point.x) + fabs(current_odom_y - home_point.y) +
              fabs(current_odom_z - home_point.z) <=
          return_home_threshold)
      {
        printf(cursclean);
        std::cout << "\033[1;32mReturn home completed\033[0m" << std::endl;
        printf(cursup);
        std_msgs::Bool stop_exploring;
        stop_exploring.data = true;
        stop_signal_pub.publish(stop_exploring);
      }
      else
      {
        while (!gp_in_progress)
        {
          ros::spinOnce();
          ros::Duration(2.0).sleep();

          graph_planner_command.command = graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION;
          graph_planner_command.location = goal_point;
          gp_command_pub.publish(graph_planner_command);
        }
      }
      ros::Duration(0.1).sleep();
    }
  }

逻辑其实很简单

5.1第一个if是用来判断机器人是否处于back_home阶段。

        如果处于back_home阶段，则判断机器人距离home点的距离是否满足阈值，若满足则当达程序停止，否则不断通过gp_command_pub.publish(graph_planner_command);来控制机器人到达home点。

else
    {
      ros::spinOnce();
      if (fabs(current_odom_x - home_point.x) + fabs(current_odom_y - home_point.y) +
              fabs(current_odom_z - home_point.z) <=
          return_home_threshold)
      {
        printf(cursclean);
        std::cout << "\033[1;32mReturn home completed\033[0m" << std::endl;
        printf(cursup);
        std_msgs::Bool stop_exploring;
        stop_exploring.data = true;
        stop_signal_pub.publish(stop_exploring);
      }
      else
      {
        while (!gp_in_progress)
        {
          ros::spinOnce();
          ros::Duration(2.0).sleep();

          graph_planner_command.command = graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION;
          graph_planner_command.location = goal_point;
          gp_command_pub.publish(graph_planner_command);
        }
      }
      ros::Duration(0.1).sleep();
    }

        如果不处于back_home阶段，首先向dsvplaner_srv服务器发送请求，数据格式为

Header header

-------

geometry_msgs/Point[] goal

std_msgs/Int32 mode

if (iteration != 0)
      {
        for (int i = 0; i < 8; i++)
        {
          printf(cursup);
          printf(cursclean);
        }
      }
      std::cout << "Planning iteration " << iteration << std::endl;
      dsvplanner::dsvplanner_srv planSrv;
      dsvplanner::clean_frontier_srv cleanSrv;
      planSrv.request.header.stamp = ros::Time::now();
      planSrv.request.header.seq = iteration;
      planSrv.request.header.frame_id = map_frame;//"/map"

其中if (iteration != 0)...是用来刷新终端窗口显示数据。

5.2第二个if (ros::service::call("drrtPlannerSrv", planSrv))是用来判断dsvplanner是否开始正常运行

        如果没有正常运行，进入while循环等待drrt planner正常运行

      else
      {
        std::cout << "Cannot call drrt planner." << std::flush;

        ros::Duration(1.0).sleep();
      }

        如果正常运行，通过判断服务器返回的goal的大小来判端planner是否启动。

        如果size=0，说明此时planner还未启动，等待启动

        如果size=1，说明启动成功

        启动成功后，判断mode的值来确定机器人的运行状态

        如果mode的值为2说明此时机器人进入back_home状态

        否则处于探索状态。

        该状态下获取goal的第一个元素作为目标点。并且开始计算此次运算时间和累积总时间。

        if (planSrv.response.goal.size() == 0)
        {  // usually the size should be 1 if planning successfully
          ros::Duration(1.0).sleep();
          continue;
        }

        if (planSrv.response.mode.data == 2)
        {
          return_home = true;
          goal_point = home_point;
          std::cout << std::endl << "\033[1;32mExploration completed, returning home\033[0m" << std::endl << std::endl;
          effective_time.data = 0;
          effective_plan_time_pub.publish(effective_time);
        }
        else
        {
          return_home = false;
          goal_point = planSrv.response.goal[0];
          plan_over = steady_clock::now();
          time_span = plan_over - plan_start;
          effective_time.data = float(time_span.count()) * steady_clock::period::num / steady_clock::period::den;
          effective_plan_time_pub.publish(effective_time);
        }
        total_time.data += effective_time.data;
        total_plan_time_pub.publish(total_time);

5.3第三个if (!simulation)根据作者描述处于simulation模式，机器人不会移动到目标点而是每两秒重新规划，应该是测试使用。

       当simulation=false时，关键在while(gp_in_progress)中，该逻辑会判断机器人20s内能否到达wayPoint点，否则删除该点及其边界。到达目标点后，机器人开始寻找下一个wayPoint。至此一个逻辑循环结束，接下来一直重复该步骤，直至探索完成回到home点。

        if (!simulation)  
        {  // when not in simulation mode, the robot will go to
           // the goal point according to graph planner
          graph_planner_command.command = graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION;
          graph_planner_command.location = goal_point;
          gp_command_pub.publish(graph_planner_command);//"/graph_planner_command" 通过这个话题把目标点发出去
          ros::Duration(dtime).sleep();  // give sometime to graph planner for
                                         // searching path to goal point
          ros::spinOnce();               // update gp_in_progree 让spinOnce去回调函数队列调用回调函数，直到gp_in_progress=true
          int count = 200;
          previous_odom_x = current_odom_x;
          previous_odom_y = current_odom_y;
          previous_odom_z = current_odom_z;
          while (gp_in_progress)
          {                            
                                         // if the waypoint keep the same for 20
                                         // (200*0.1)
            ros::Duration(0.1).sleep();  // seconds, then give up the goal
            wayPoint_pre = wayPoint;
            ros::spinOnce();
            bool robotMoving = robotPositionChange();
            if (robotMoving)
            {

              count = 200;
            }
            else
            {
              count--;
            }
            if (count <= 0)
            {  // when the goal point cannot be reached, clean
               // its correspoinding frontier if there is
              cleanSrv.request.header.stamp = ros::Time::now();
              cleanSrv.request.header.frame_id = map_frame;
              ros::service::call("cleanFrontierSrv", cleanSrv);
              ros::Duration(0.1).sleep();
              break;
            }
          }

          graph_planner_command.command = graph_planner::GraphPlannerCommand::COMMAND_DISABLE;
          gp_command_pub.publish(graph_planner_command);
        }

6补充

6.1 gp_status_callback

/*
  接受/graph_planner_status话题的消息
  类型为为:graph_planner::GraphPlannerStatus（自定义）
  数据格式为：uint8 status
            uint8 STATUS_DISABLED=0
            uint8 STATUS_IN_PROGRESS=1
  若为1则处于探索阶段 使gp_in_progress=True
*/
void gp_status_callback(const graph_planner::GraphPlannerStatus::ConstPtr& msg)
{
  if (msg->status == graph_planner::GraphPlannerStatus::STATUS_IN_PROGRESS)
    gp_in_progress = true;
  else
  {
    gp_in_progress = false;
  }
}

6.2 waypoint_callback

/*
  接受/way_point话题的消息
  类型为:geometry_msgs::PointStamped
  格式为:Header header
        Point point
  获取目标点坐标，这里是自发自收，给一个初始的目标点运行 x=4 y=0 z=0
*/
void waypoint_callback(const geometry_msgs::PointStamped::ConstPtr& msg)
{
  wayPoint = msg->point;
  wp_state = true; 

}

6.3 odom_callback

/*
  接受/state_estimation话题的消息
  类型为:nav_msgs::Odometry
  格式为:Header header
        string child_frame_id
        geometry_msgs/PoseWithCovariance pose
        geometry_msgs/TwistWithCovariance twist
  将接收到的消息tf转化到map坐标下
*/
void odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
  current_odom_x = msg->pose.pose.position.x;
  current_odom_y = msg->pose.pose.position.y;
  current_odom_z = msg->pose.pose.position.z;

  transformToMap.setOrigin(
      tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z));
  transformToMap.setRotation(tf::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y,
                                            msg->pose.pose.orientation.z, msg->pose.pose.orientation.w));
}

6.4 begin_signal_callback

/*
  接受/start_exploring话题的消息
  类型为:std_msgs::Bool
  将接收到的消息保存到begin_signal中
*/
void begin_signal_callback(const std_msgs::Bool::ConstPtr& msg)
{
  begin_signal = msg->data;
}

6.5 robotPositionChange

/*
  判断是否可以移动到目标点
*/
bool robotPositionChange()
{
  double dist = sqrt((current_odom_x - previous_odom_x) * (current_odom_x - previous_odom_x) +
                     (current_odom_y - previous_odom_y) * (current_odom_y - previous_odom_y) +
                     (current_odom_z - previous_odom_z) * (current_odom_z - previous_odom_z));
  if (dist < robot_moving_threshold)
    return false;
  previous_odom_x = current_odom_x;
  previous_odom_y = current_odom_y;
  previous_odom_z = current_odom_z;
  return true;
}

7 总结

该节点为上层节点，负责总体逻辑，并没有具体实现。