GazeboRosControlPlugin::Load 函数详解

本文涉及的产品
资源编排,不限时长
简介: 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.

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

相关实践学习
使用ROS创建VPC和VSwitch
本场景主要介绍如何利用阿里云资源编排服务,定义资源编排模板,实现自动化创建阿里云专有网络和交换机。
阿里云资源编排ROS使用教程
资源编排(Resource Orchestration)是一种简单易用的云计算资源管理和自动化运维服务。用户通过模板描述多个云计算资源的依赖关系、配置等,并自动完成所有资源的创建和配置,以达到自动化部署、运维等目的。编排模板同时也是一种标准化的资源和应用交付方式,并且可以随时编辑修改,使基础设施即代码(Infrastructure as Code)成为可能。 产品详情:https://www.aliyun.com/product/ros/
相关文章
|
存储 JSON 数据格式
数据集加载时报错'dict' object has no attribute 'requests‘
数据集加载时报错'dict' object has no attribute 'requests‘
277 5
|
6月前
|
机器学习/深度学习 监控 物联网
函数计算操作报错合集之调用接口提示Cannot copy out of meta tensor; no data! 是什么原因
在使用函数计算服务(如阿里云函数计算)时,用户可能会遇到多种错误场景。以下是一些常见的操作报错及其可能的原因和解决方法,包括但不限于:1. 函数部署失败、2. 函数执行超时、3. 资源不足错误、4. 权限与访问错误、5. 依赖问题、6. 网络配置错误、7. 触发器配置错误、8. 日志与监控问题。
215 0
|
3月前
|
Rust 编译器 C++
使用 def、cdef、cpdef 创建函数
使用 def、cdef、cpdef 创建函数
47 0
|
7月前
|
JSON 数据格式 Python
失之毫厘差之千里之load和loads
最近在读pandas库的一些文档的时候,顺便也会将文档上的一些demo在编辑器中进行运行测试,其中在读到pandas处理Json数据这一节的时候,我还是像往常一样,将文档提供的demo写一遍,结果在运行的时候,直接报异常了,我们来看看我的代码和报错信息吧!
|
安全 Python
YAML+PyYAML笔记 8 | PyYAML源码之full_load(),full_load_all(),safe_load(),unsafe_load(),unsafe_load_all()
YAML+PyYAML笔记 8 | PyYAML源码之full_load(),full_load_all(),safe_load(),unsafe_load(),unsafe_load_all()
135 1
|
7月前
|
机器学习/深度学习 PyTorch 算法框架/工具
通过实例学习Pytorch加载权重.load_state_dict()与保存权重.save()
通过实例学习Pytorch加载权重.load_state_dict()与保存权重.save()
89 0
|
机器学习/深度学习 PyTorch 算法框架/工具
|
并行计算 算法 Go
如何调用一个只支持batch_call的服务?
如何调用一个只支持batch_call的服务?
52 0
|
缓存 Python
Load和Initialize的区别和使用
Load和Initialize的区别和使用
186 0