GazeboRosControlPlugin::Load 函数详解

void GazeboRosControlPlugin::Load（）

函数源码(已加注释)

// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
  ROS_INFO_STREAM_NAMED("gazebo_ros_control","Loading gazebo_ros_control plugin");

  // Save pointers to the model
  parent_model_ = parent;//保存指向模型的指针
  sdf_ = sdf;

  // Error message if the model couldn't be found
  if (!parent_model_)//模型指针为空
  {
  //返回
    ROS_ERROR_STREAM_NAMED("loadThread","parent model is NULL");
    return;
  }

  // Check that ROS has been initialized
  if(!ros::isInitialized())//如果ros没有初始化完成
  {
    //打印信息 并返回 
    ROS_FATAL_STREAM_NAMED("gazebo_ros_control","A ROS node for Gazebo has not been initialized, unable to load plugin. "
      << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

  // 从 sdf_ 里提取信息
  // sdf_  应该就是gazebo的标签
  // 里面会有 robotNamespace robotSimType controlPeriod  等子标签 
  // 下面代码主要对里面的信息进行提取 ，并作相应处理

  // Get namespace for nodehandle
  if(sdf_->HasElement("robotNamespace"))
  {
      // 如果有 robotNamespace 这个标签 那提取这个标签的内容
    robot_namespace_ = sdf_->GetElement("robotNamespace")->Get<std::string>();
  }
  else
  {
     //如果没有 则 以机器人名称作为robot_namespace_
    robot_namespace_ = parent_model_->GetName(); // default  //如果没有 则 以机器人名称作为robot_namespace_
  }

  // Get robot_description ROS param name
  // 得到参数服务器中 robot_description 
  // ！！！ 当用到多个机器人的时候，那么可能要注意这个地方
  if (sdf_->HasElement("robotParam"))
  {
  // 如果有那么把  robotParam 标签的内容 给 robot_description_
    robot_description_ = sdf_->GetElement("robotParam")->Get<std::string>();
  }
  else
  { 
  //  没有  robotParam 标签   robot_description_ 直接为 "robot_description"
    robot_description_ = "robot_description"; // default
  }
  
  // Get the robot simulation interface type
  // 读取 robotSimType 标签的值 也就是加载默认的接口还是 用户自定义的接口
  if(sdf_->HasElement("robotSimType"))
  {
  // 有这个标签  则把标签里的值 存到 robot_hw_sim_type_str_
    robot_hw_sim_type_str_ = sdf_->Get<std::string>("robotSimType");
  }
  else
  {   //没有这个标签 则使用默认的接口  
    robot_hw_sim_type_str_ = "gazebo_ros_control/DefaultRobotHWSim";
    ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<<robot_hw_sim_type_str_<<"\"");
  }

  

  // temporary fix to bug regarding the robotNamespace in default_robot_hw_sim.cpp (see #637)
  std::string robot_ns = robot_namespace_;
  if(robot_hw_sim_type_str_ == "gazebo_ros_control/DefaultRobotHWSim"){
      if (sdf_->HasElement("legacyModeNS")) {
          if( sdf_->GetElement("legacyModeNS")->Get<bool>() ){
              robot_ns = "";
          }
      }else{
          robot_ns = "";
          ROS_ERROR("GazeboRosControlPlugin missing <legacyModeNS> while using DefaultRobotHWSim, defaults to true.\n"
                    "This setting assumes you have an old package with an old implementation of DefaultRobotHWSim, "
                    "where the robotNamespace is disregarded and absolute paths are used instead.\n"
                    "If you do not want to fix this issue in an old package just set <legacyModeNS> to true.\n"
                    );
      }
  }

  // Get the Gazebo simulation period
#if GAZEBO_MAJOR_VERSION >= 8
  ros::Duration gazebo_period(parent_model_->GetWorld()->Physics()->GetMaxStepSize());
#else
// 得到gazebo 的 更新时间
  ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize());
#endif

  // Decide the plugin control period
  if(sdf_->HasElement("controlPeriod"))
  {
    control_period_ = ros::Duration(sdf_->Get<double>("controlPeriod"));

    // Check the period against the simulation period
    if( control_period_ < gazebo_period )
    {
      ROS_ERROR_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
        <<" s) is faster than the gazebo simulation period ("<<gazebo_period<<" s).");
    }
    else if( control_period_ > gazebo_period )
    {
      ROS_WARN_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
        <<" s) is slower than the gazebo simulation period ("<<gazebo_period<<" s).");
    }
  }
  else
  {// 没有  controlPeriod  这个标签 则 默认使用 gazebo 的 更新频率
    control_period_ = gazebo_period;
    ROS_DEBUG_STREAM_NAMED("gazebo_ros_control","Control period not found in URDF/SDF, defaulting to Gazebo period of "
      << control_period_);
  }

  // Get parameters/settings for controllers from ROS param server
  model_nh_ = ros::NodeHandle(robot_namespace_);

  // Initialize the emergency stop code.
  e_stop_active_ = false;
  last_e_stop_active_ = false;
  if (sdf_->HasElement("eStopTopic"))
  {
    const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get<std::string>();
    e_stop_sub_ = model_nh_.subscribe(e_stop_topic, 1, &GazeboRosControlPlugin::eStopCB, this);
  }

  ROS_INFO_NAMED("gazebo_ros_control", "Starting gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str());

  // Read urdf from ros parameter server then
  // setup actuators and mechanism control node.
  // This call will block if ROS is not properly initialized.
  const std::string urdf_string = getURDF(robot_description_);
  if (!parseTransmissionsFromURDF(urdf_string))
  {
    ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n");
    return;
  }

  // Load the RobotHWSim abstraction to interface the controllers with the gazebo model
  try
  {
  // 通过ClassLoader加载RobotHWSim
    robot_hw_sim_loader_.reset
      (new pluginlib::ClassLoader<gazebo_ros_control::RobotHWSim>
        ("gazebo_ros_control",
          "gazebo_ros_control::RobotHWSim"));

    // 加载 robotSimType 下的那个插件
    robot_hw_sim_ = robot_hw_sim_loader_->createInstance(robot_hw_sim_type_str_);
    urdf::Model urdf_model;
    const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;

    // 调用initSim函数  
    if(!robot_hw_sim_->initSim(robot_ns, model_nh_, parent_model_, urdf_model_ptr, transmissions_))
    {
      ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface");
      return;
    }

    // Create the controller manager
    // 创建ControllerManager, 可以看到， ControllerManager是在这个地方被初始化。
    ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
    controller_manager_.reset
      (new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_));

    // Listen to the update event. This event is broadcast every simulation iteration.
    // 监听update event, 每个simulation iteration都会广播该event.
    update_connection_ =
      gazebo::event::Events::ConnectWorldUpdateBegin
      (boost::bind(&GazeboRosControlPlugin::Update, this));

  }
  catch(pluginlib::LibraryLoadException &ex)
  {
    ROS_FATAL_STREAM_NAMED("gazebo_ros_control","Failed to create robot simulation interface loader: "<<ex.what());
  }

  ROS_INFO_NAMED("gazebo_ros_control", "Loaded gazebo_ros_control.");
}

代码功能块解读

====================================================================================================

容错判断，模型指针不能为空。 ros 要完成初始化

=========================================================================================================
从 sdf_ 里提取信息
sdf_ 应该就是gazebo的标签
里面会有 robotNamespace robotSimType controlPeriod 等子标签
下面代码主要对里面的信息进行提取 ，并作相应处理

robotNamespace：
如果有 robotNamespace 这个标签 那提取这个标签的内容
如果没有 则 以机器人名称作为robot_namespace_

robotParam:
如果有那么把 robotParam 标签的内容 给 robot_description_
没有 robotParam 标签 robot_description_ 直接为 "robot_description"

设定了robotNamespace 标签， 没有设定 robot_description 标签。

没有设定 robotParam 标签，那么 robot_description_ 直接为 "robot_description" 这是什么流程：
在本函数后面的代码中有 robot_description_ 的如下内容

也就是说需要从参数服务器中根据 robot_description_ 找urdf对应的 模型
在launch 文件中 一般会在参数服务器中定义 robot_description 与 urdf的对应关系

所以应该在设置的时候 要注意 robotParam 便签与 上面两个红框对应

• [ ] 测试不对应时是怎样的

=========================================================================================================

然后是robotSimType标签 此处也就是加载自定义的还是 默认的
没有声明robotSimType标签，则加载默认的标签 此处说的很清楚

直接设置了 默认的标签 与不设置效果一样
需要加载自己的标签那么 需要改成自己的

=========================================================================================================

controlPeriod 控制周期
读取gazebo的更新周期，与控制周期比较。 进行相应提示， 控制周期小于 更新周期没事

=========================================================================================================

通过ClassLoader加载RobotHWSim

=========================================================================================================

加载 robotSimType 下的那个插件

=========================================================================================================

调用initSim函数
也就是 默认或者自己插件写的那个

=========================================================================================================
创建ControllerManager, 可以看到， ControllerManager是在这个地方被初始化
监听update event, 每个simulation iteration都会广播该event.

=========================================================================================================