ROS：：重置gmapping的方法

#ROS：： 重置gmapping的方法

代码链接：https://download.csdn.net/download/qq_14977553/12326447
有时候需要让gmapping重置，通过杀死节点的方式太粗暴，这里提供一种让gmapping重置的方法
1、首先编写一个重置gmapping的服务，通过该服务可以查询gmapping状态是否是正在运行，服务如下：

uint8 cmd
---
uint8 status

字段说明：cmd = 1,表示重置gmapping
cmd = 2,表示查询gmapping的状态
status = 0,表示gmapping没有运行
status = 1,表示gmapping正在运行
2、修改main.cpp：
修改之前：

#include <ros/ros.h>

#include "slam_gmapping.h"

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "slam_gmapping");

  SlamGMapping gn;
  gn.startLiveSlam();
  ros::spin();

  return(0);
}

修改之后：

#include <ros/ros.h>
#include "slam_gmapping.h"
#include "robot_srv/SlamCmd.h" //自己写的服务，根据自己服务修改头文件

ros::ServiceServer service_;

uint8_t restart_flag_;
bool isSlamOk_;

boost::mutex mutex_;

bool slamSrvCallback(robot_srv::SlamCmd::Request& request, robot_srv::SlamCmd::Response& response);

int main(int argc, char** argv)
{
  isSlamOk_ = false;
  restart_flag_ = 0;
  ros::init(argc, argv, "slam_gmapping");
  ros::NodeHandle slam_nh("~");
  SlamGMapping gn;
  gn.startLiveSlam();

  service_ = slam_nh.advertiseService("/slam_cmd_srv", slamSrvCallback); // 命令服务

  ros::Rate rate(20);
  while (ros::ok())
  {
    if(restart_flag_ != 0)
    {
      restart_flag_ = 0;
      gn.restart();
      ros::Duration(0.3).sleep();
      isSlamOk_ = true;
    }
    ros::MultiThreadedSpinner s(3);
    ros::spinOnce();
    rate.sleep();
  }

  service_.shutdown();

  return(0);
}

bool slamSrvCallback(robot_srv::SlamCmd::Request& request, robot_srv::SlamCmd::Response& response)
{
  ROS_WARN("slamSrvCallback request:%d",request.cmd);
  switch (request.cmd)
  {
    case 1:
    {
      boost::mutex::scoped_lock lock(mutex_);
      restart_flag_ = 1;
      isSlamOk_ = false;
      response.status = isSlamOk_;
      break;
    }
    case 2:
    {
      boost::mutex::scoped_lock lock(mutex_);
      response.status = isSlamOk_;
      break;
    }
    default:
      break;
  }
  return true;
}

3、修改void SlamGMapping::startLiveSlam()：
修改之前

void SlamGMapping::startLiveSlam()
{
  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);

  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

修改之后

void SlamGMapping::startLiveSlam()
{
  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);

//  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
//  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
//  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
//  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

4、添加SlamGMapping::restart()函数：

void SlamGMapping::restart()
{
  if(transform_thread_)
  {
    delete transform_thread_;
    transform_thread_ = NULL;
  }

  if(scan_filter_)
  {
    delete scan_filter_;
    scan_filter_ = NULL;
  }

  if(scan_filter_sub_)
  {
    delete scan_filter_sub_;
    scan_filter_sub_ = NULL;
  }

  if(gsp_)
  {
    delete gsp_;
    gsp_ = NULL;
  }

  map_to_odom_= tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ));
  laser_count_ = 0;

  seed_ = time(NULL);

  gsp_ = new GMapping::GridSlamProcessor();
  ROS_ASSERT(gsp_);

  if(gsp_laser_)
  {
    delete gsp_laser_;
    gsp_laser_ = NULL;
  }
  if(gsp_odom_)
  {
    delete gsp_odom_;
    gsp_odom_ = NULL;
  }

  got_first_scan_ = false;

  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

5、说明：启动时gmapping会卡住等着重置信息，收到消息后再启动建图和定位。
gmapping正常运行中通过调用该服务设置cmd = 1即可重置gmapping。

修改后的代码下载地址
https://download.csdn.net/download/qq_14977553/12326447