ros中创建静态ros::ServiceServer可确保服务只在第一个类实例创建时初始化

ros中创建静态ros::ServiceServer可确保服务只在第一个类实例创建时初始化

业务中move_base_flex做导航到，但是我要实现暂停功能，我就想改AbstractControllerExecution这个代码，当我这个代码中收到我发送的消息时就暂停/cmd_vel数据的发出，而这个mbf_abstract_nav::AbstractControllerExecution这类在导航中会一直创建实例，直接在这个类中查创建ros::ServiceServer 消息时控制台会一直报错：Tried to advertise a service that is already advertised in this node [/pause_cmd_vel]

为了确保在AbstractControllerExecution类中只初始化一次，可以使用静态变量确保服务只在第一个类实例创建时初始化。

首先，在AbstractControllerExecution类的声明中，将pause_cmd_vel_service_声明为静态成员变量：

//sukai
#include <std_srvs/SetBool.h>
class AbstractControllerExecution
{
  // ...
  protected:
        //  控制/cmd_vel 这个话题的暂停和启动
    static ros::ServiceServer pause_cmd_vel_service_;
    static bool pause_cmd_vel_;
    //回调函数pauseCmdVelCB只需要在头文件.h中声明 static ;
    static bool pauseCmdVelCB(std_srvs::SetBool::Request &req, 
    std_srvs::SetBool::Response &res);
    // ...
};

然后，在AbstractControllerExecution类的源文件中定义静态变量：

ros::ServiceServer AbstractControllerExecution::pause_cmd_vel_service_;
bool AbstractControllerExecution::pause_cmd_vel_;

#include <mbf_msgs/ExePathResult.h>

#include "mbf_abstract_nav/abstract_controller_execution.h"

namespace mbf_abstract_nav
{

//在AbstractControllerExecution类的源文件中定义静态变量：
ros::ServiceServer AbstractControllerExecution::pause_cmd_vel_service_;
bool AbstractControllerExecution::pause_cmd_vel_;

...

//构造函数
AbstractControllerExecution::AbstractControllerExecution(){
...
}
...

}

最后，在类的构造函数中使用静态变量is_service_initialized初始化服务，同时确保仅进行一次初始化：

AbstractControllerExecution::AbstractControllerExecution(
    const std::string &name,
    const mbf_abstract_core::AbstractController::Ptr &controller_ptr,
    const mbf_utility::RobotInformation &robot_info,
    const ros::Publisher &vel_pub,
    const ros::Publisher &goal_pub,
    const MoveBaseFlexConfig &config) :
  AbstractExecutionBase(name, robot_info),
    controller_(controller_ptr),
    state_(INITIALIZED), moving_(false), max_retries_(0), patience_(0), vel_pub_(vel_pub), current_goal_pub_(goal_pub),
    loop_rate_(DEFAULT_CONTROLLER_FREQUENCY)
{
  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  // non-dynamically reconfigurable parameters
  private_nh.param("robot_frame", robot_frame_, std::string("base_link"));
  private_nh.param("map_frame", global_frame_, std::string("map"));
  private_nh.param("force_stop_at_goal", force_stop_at_goal_, false);
  private_nh.param("force_stop_on_cancel", force_stop_on_cancel_, false);
  private_nh.param("mbf_tolerance_check", mbf_tolerance_check_, false);
  private_nh.param("dist_tolerance", dist_tolerance_, 0.1);
  private_nh.param("angle_tolerance", angle_tolerance_, M_PI / 18.0);
  private_nh.param("tf_timeout", tf_timeout_, 1.0);

//在类的构造函数中使用静态变量is_service_initialized初始化服务，同时确保仅进行一次初始化   
static bool is_service_initialized = false;
  if (!is_service_initialized)
  {
   pause_cmd_vel_ = false;
   pause_cmd_vel_service_ = nh.advertiseService("pause_cmd_vel", &AbstractControllerExecution::pauseCmdVelCB);
  is_service_initialized = true;
  }


  reconfigure(config);
}

这样，pause_cmd_vel_service_将在类的第一个实例化时初始化，而后续实例化将不会重复初始化。由于该服务仅在第一个对象创建时进行了一次广告，因此您不会收到Tried to advertise a service that is already advertised in this node [/pause_cmd_vel]的错误消息。

注意：pause_cmd_vel_service_ = nh.advertiseService("pause_cmd_vel", &AbstractControllerExecution::pauseCmdVelCB);这里末尾不需要传入this变量；

回调函数：

回调函数pauseCmdVelCB只需要在头文件.h中声明 static ;

 bool AbstractControllerExecution::pauseCmdVelCB(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
  pause_cmd_vel_ = req.data;
  res.success = true;
  res.message = req.data ? "Pausing /cmd_vel topic." : "Resuming /cmd_vel topic.";
  return true;
}

以上就是ros中创建静态ros::ServiceServer可确保服务只在第一个类实例创建时初始化；

----

void AbstractControllerExecution::run()中409行增加以下代码实现暂停活启动小车，原理是控制/cmd_vel,也可以在里程计中控制/cmd_vel达到同样的效果；

if (pause_cmd_vel_)
	{
	  publishZeroVelocity();
	   //sukai  控制/cmd_vel 这个话题的暂停和启动
	   continue;
	}

 publishZeroVelocity();函数

void AbstractControllerExecution::publishZeroVelocity()
{
  geometry_msgs::Twist cmd_vel;
  cmd_vel.linear.x = 0;
  cmd_vel.linear.y = 0;
  cmd_vel.linear.z = 0;
  cmd_vel.angular.x = 0;
  cmd_vel.angular.y = 0;
  cmd_vel.angular.z = 0;
  vel_pub_.publish(cmd_vel);
}