DefaultRobotHWSim::initSim()

源码（加了注释）

bool DefaultRobotHWSim::initSim(
  const std::string& robot_namespace,
  ros::NodeHandle model_nh,
  gazebo::physics::ModelPtr parent_model,
  const urdf::Model *const urdf_model,
  std::vector<transmission_interface::TransmissionInfo> transmissions)
{
  // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each
  // parameter's name is "joint_limits/<joint name>". An example is "joint_limits/axle_joint".
  const ros::NodeHandle joint_limit_nh(model_nh);

  // Resize vectors to our DOF
  // transmissions个数即joint 个数 在xacro文件里一个transmissions标签对应一个joint
  n_dof_ = transmissions.size();
  //根据joint个数为各变量申请内存空间
  joint_names_.resize(n_dof_);
  joint_types_.resize(n_dof_);
  joint_lower_limits_.resize(n_dof_);
  joint_upper_limits_.resize(n_dof_);
  joint_effort_limits_.resize(n_dof_);
  joint_control_methods_.resize(n_dof_);
  pid_controllers_.resize(n_dof_);
  joint_position_.resize(n_dof_);
  joint_velocity_.resize(n_dof_);
  joint_effort_.resize(n_dof_);
  joint_effort_command_.resize(n_dof_);
  joint_position_command_.resize(n_dof_);
  joint_velocity_command_.resize(n_dof_);

  // Initialize values  初始化所有变量
  for(unsigned int j=0; j < n_dof_; j++)
  {
    // Check that this transmission has one joint
    // 检查  transmission  标签里是几个joint
    if(transmissions[j].joints_.size() == 0)
    { 
        //没有joint即 提示 。。。
      ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_  << " has no associated joints.");
      continue;
    }
    else if(transmissions[j].joints_.size() > 1) //多余1个joint即 提示 。。。
    {
      ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
        << " has more than one joint. Currently the default robot hardware simulation "
        << " interface only supports one.");
      continue;
    }
    //提取transmissions标签里得joint下得 hardware_interfaces_ 
    std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
        //如果没有joint 下得hardware_interfaces_ 但是有actuators_ 下得hardware_interfaces_
    if (joint_interfaces.empty() &&
        !(transmissions[j].actuators_.empty()) &&
        !(transmissions[j].actuators_[0].hardware_interfaces_.empty()))
    {   
      // TODO: Deprecate HW interface specification in actuators in ROS J
        //不算错，将actuators_ 下得hardware_interfaces_ 赋值给joint_interfaces继续运行代码
      joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
      // 提示...
      ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The <hardware_interface> element of tranmission " <<
        transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
        "The transmission will be properly loaded, but please update " <<
        "your robot model to remain compatible with future versions of the plugin.");
    }
       //如果还没有则 出错，提示  退出该transmissions[j]
    if (joint_interfaces.empty())
    {
      ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
        " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
        "Not adding it to the robot hardware simulation.");
      continue;
    }
      //如果 joint下得 hardware_interfaces_ 多于1个
    else if (joint_interfaces.size() > 1)
    {  // 提示 但步算错，使用第1个
      ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
        " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
        "Currently the default robot hardware simulation interface only supports one. Using the first entry");
      //continue;
    }

    // Add data from transmission
    // 初始化各变量 name, pos, vel, eff, 以及cmd
    joint_names_[j] = transmissions[j].joints_[0].name_;
    joint_position_[j] = 1.0;
    joint_velocity_[j] = 0.0;
    joint_effort_[j] = 1.0;  // N/m for continuous joints
    joint_effort_command_[j] = 0.0;
    joint_position_command_[j] = 0.0;
    joint_velocity_command_[j] = 0.0;

     // 常量引用 仅用第一个
    const std::string& hardware_interface = joint_interfaces.front();

    // Debug  // 提示加载了 joint 及 它得hardware_interface
    ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j]
      << "' of type '" << hardware_interface << "'");

    // 下面用到了很多 
    // hardware_interface:: JointHandle JointStateInterface EffortJointInterface PositionJointInterface VelocityJointInterface
   // 里的功能函数    
   // 要是自己写 硬件接口 需要研究此部分内容 
   
    // Create joint state interface for all joints
    // 为每个joint 绑定对应得状态（三个：positionelocity、effort）
    // ！！！具体怎么操作得需要看hardware_interface里 JointStateHandle源码 应该是一个写好得类 处理硬件接口
    //  js_interface_ 也是 hardware_interface::JointStateInterface 类得实例化
    js_interface_.registerHandle(hardware_interface::JointStateHandle(
        joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));

    // Decide what kind of command interface this actuator/joint has
    hardware_interface::JointHandle joint_handle;//声明 joint handle
     //根据 transmissions  标签中 hardware_interface  的类型 注册不通过joint 接口
    if(hardware_interface == "EffortJointInterface" || hardware_interface == "hardware_interface/EffortJointInterface")
    {//处理 力控制joint
       //此部分具体实现要看 hardware_interface::JointHandle 源码 
   
      // Create effort joint interface
      joint_control_methods_[j] = EFFORT; //
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_effort_command_[j]);

       //ej_interface_ 是hardware_interface::EffortJointInterface类的实例化
       // 具体实现内容要看hardware_interface::EffortJointInterface
        //注册   effort joint interface ：将 joint_handle 注册到 ej_interface_ 相当于创建一个ej_interface_
      ej_interface_.registerHandle(joint_handle);//注册   effort joint interface 
    }
    else if(hardware_interface == "PositionJointInterface" || hardware_interface == "hardware_interface/PositionJointInterface")
    {//处理位置控制joint
      //此部分具体实现要看 hardware_interface::JointHandle 源码 
      // Create position joint interface
      joint_control_methods_[j] = POSITION;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_position_command_[j]);

       //pj_interface_ 是hardware_interface::PositionJointInterface类的实例化
       // 具体实现内容要看hardware_interface::PositionJointInterface 
       //注册   position joint interface  ：将 joint_handle 注册到 pj_interface_ 相当于创建一个pj_interface_
      pj_interface_.registerHandle(joint_handle);  ;
    }
    else if(hardware_interface == "VelocityJointInterface" || hardware_interface == "hardware_interface/VelocityJointInterface")
    { //处理速度控制joint
     //此部分具体实现要看 hardware_interface::JointHandle 源码 
      // Create velocity joint interface
      joint_control_methods_[j] = VELOCITY;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_velocity_command_[j]);

        //vj_interface_ 是hardware_interface::VelocityJointInterface类的实例化
         // 具体实现内容要看hardware_interface::VelocityJointInterface    
                //注册   velocity joint interface ：将 joint_handle 注册到 vj_interface_ 相当于创建一个vj_interface_
      vj_interface_.registerHandle(joint_handle);//注册   velocity joint interface
    }
    else
    {// 必须是其中三种的一种 否则 报错提示 并返回 未初始化成功
      ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '"
        << hardware_interface << "' while loading interfaces for " << joint_names_[j] );
      return false;
    }

    // 如果 hardware_interface 的标签 没有加 hardware_interface/   就建议加上
    if(hardware_interface == "EffortJointInterface" || hardware_interface == "PositionJointInterface" || hardware_interface == "VelocityJointInterface") {
      ROS_WARN_STREAM("Deprecated syntax, please prepend 'hardware_interface/' to '" << hardware_interface << "' within the <hardwareInterface> tag in joint '" << joint_names_[j] << "'.");
    }
   
     //对应gazebo的joint与robot 的joint
     //gazebo里的应该是urdf或xacro里声明的
     //robot 的joint 应该是  transmissions  里的 
     // 所以在写xacro的时候一定要对应上   urdf或xacro里声明的joint 可以不写transmissions  但是transmissions里的joint一定要在xacro里写
     
    // Get the gazebo joint that corresponds to the robot joint.
    //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: "
    //  << joint_names_[j]);
    // 从 gazebo里面 查找对应的  transmissions  里的 joint
    //  ！！！可以实验下
    gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
    if (!joint)//如果没有找到 该joint的话
    {
    // 报错  ： default_robot_hw 插件 ，机器人有一个joint：（joint的名字），在gazebo里没有找到
    //  就是 transmissions  里的 joint  没有在 在gazebo里（urdf或xacro里声明的）没有找到
    //并返回  false 未初始化成功
      ROS_ERROR_STREAM_NAMED("default_robot_hw", "This robot has a joint named \"" << joint_names_[j]
        << "\" which is not in the gazebo model.");
      return false;
    }
    
    // sim_joints_声明类型是   std::vector<gazebo::physics::JointPtr>
     // 可以看 vector 的 push_back 作用是什么  
    sim_joints_.push_back(joint);

    // get physics engine type
#if GAZEBO_MAJOR_VERSION >= 8
    gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics();
#else
   // 得到gazebo得物理引擎模式  为write（）提供选择 一般为ode  ODE(Open Dynamics Engine)  gazebo 得模式 可以先不管  应该是 ode模式
    gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine();
#endif
   //!!!打印看下
    physics_type_ = physics->GetType();   // 得到gazebo得物理引擎模式    应该是 ode模式  
    if (physics_type_.empty())
    {
      ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "No physics type found.");
    }

    // 注册 joint 限制 初始化  是本类的函数  具体功能到那个函数再看
    registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j],
                        joint_limit_nh, urdf_model,
                        &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j],
                        &joint_effort_limits_[j]);

    //  joint的控制方法 是  POSITION 或者 VELOCITY 的 进行pid控制， 下面进行该控制器的初始化
    if (joint_control_methods_[j] != EFFORT)
    {
      // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
      // joint->SetParam("vel") to control the joint.
      const ros::NodeHandle nh(robot_namespace + "/gazebo_ros_control/pid_gains/" +
                               joint_names_[j]);
      if (pid_controllers_[j].init(nh)) // 进行 pid_controllers_ 的初始化  具体应该看control_toolbox::Pid 的init 功能函数
      {
        switch (joint_control_methods_[j])
        {
          case POSITION:
            joint_control_methods_[j] = POSITION_PID; // 控制方法：位置pid控制
            break;
          case VELOCITY:
            joint_control_methods_[j] = VELOCITY_PID; // 控制方法：速度pid控制
            break;
        }
      }
      else
      {
        // joint->SetParam("fmax") must be called if joint->SetAngle() or joint->SetParam("vel") are
        // going to be called. joint->SetParam("fmax") must *not* be called if joint->SetForce() is
        // going to be called.
#if GAZEBO_MAJOR_VERSION > 2

            // 是力控制 joint 的话 ，就设置力的上下限
        joint->SetParam("fmax", 0, joint_effort_limits_[j]);
#else
        joint->SetMaxForce(0, joint_effort_limits_[j]);
#endif
      }
    }
  }

  // Register interfaces
  // 注册 接口  是继承来的功能函数   父类RobotHW是继承于InterfaceManager的 
  // 具体功能应该再看 继承类的 代码   实则是调用InterfaceManager的方法。
  // ？？？  注册完干啥了 目前还不知道 后面也没用到  ？？？
  // 后面用的 sim_joints_ 作为 gazebo的joint接口
  registerInterface(&js_interface_); 
  registerInterface(&ej_interface_);
  registerInterface(&pj_interface_);
  registerInterface(&vj_interface_);

  // Initialize the emergency stop code.
  // 初始化紧急停止的参数
  e_stop_active_ = false;
  last_e_stop_active_ = false;

  // 都完成了则初始化成功
  return true;
}

对源码进行功能分块解读

由上至下

==============================================================
在这里插入图片描述
读取xacro文件中 transmissions 标签的个数，正常的xacro文件里面一个transmissions 对应一个joint。
然后根据joint个数为各变量申请内存空间
申请内存空间的操作，实际上是vector:: resize()函数，具体可参考

例如下面的图片就是博客用到xacro的截图。
里面 transmissions 标签实例
在这里插入图片描述

[ ] 可以打印看看 n_dof_ 究竟是几

=================================================================
在这里插入图片描述
上面有个for循环，遍历了每个transmissions 便签，下面不再赘述。

该部分代码主要进行个容错判断：判断 transmissions 标签下面joint的个数，

当transmissions 标签下面joint的个数等于0，也就是没有 joint 时，则提示 transmissions 的名字，和没有joint 标签。然后 continue 进行下个transmissions 标签的处理，本次的则初始化失败了。

[ ] 可以实验下

当transmissions 标签下面joint的个数大于1时，则提示 transmissions 的名字，和joint 标签个数多于1个。然后 continue 进行下个transmissions 标签的处理，本次的则初始化失败了。

[ ] 可以实验下

总结:所以要想要在gazebo里控制一个joint ，则在 transmissions 标签里一定要仅有1个joint 。

实际控制例子中博客里的 transmissions 标签下面joint标签。一般都是一个。
在这里插入图片描述

=================================================================
在这里插入图片描述
也是个容错判断，判断 transmissions 标签下面joint标签里的hardwareInterface便签使用是否正确。

std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
提取transmissions标签里得joint下得 hardwareinterfaces 为什么这么提取就要看transmission_interface::TransmissionInfo了，其实不用管。

第一个if ，如果joint标签里 hardwareInterface没有，并且有actuators便签，并且actuators便签有hardwareInterface。就把actuators便签的hardwareInterface当作joint标签的hardwareInterface。
不算错，继续执行该transmissions

第二个if，如果还是没有hardwareInterface，说明actuators也没有补充上，则提示该transmissions的名字和joint下没有hardwareInterface。并continue 进行下个transmissions 标签的处理，本次的则初始化失败了。

然后else if 是当 hardwareInterface 的个数大于1个时
提示transmissions的名字和joint下hardwareInterface个数大于一个。但是并不算错，程序继续执行。把第一个当作hardwareInterface。

实际控制例子中博客里的 transmissions 标签下面joint标签和actuator标签。一般都是一个然后hardwareInterface相同。
在这里插入图片描述
总结：transmissions 标签下面joint标签和actuator标签最好都要有，然后hardwareInterface标签一致即可。

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

在这里插入图片描述
初始化各变量

=========================================================================
在这里插入图片描述
为每个joint 绑定对应得状态（三个：positionelocity、effort）

[ ] 具体怎么操作得需要看hardware_interface里 JointStateHandle源码应该是一个写好得类处理硬件接口

js_interface_ 也是 hardware_interface::JointStateInterface 类得实例化

=========================================================================
在这里插入图片描述
如果hardwareinterface是EffortJointInterface，初始化joint_control_methods_为EFFORT
注册 effort joint interface
注册 effort joint interface ：将 joint_handle 注册到 ej_interface_ 相当于创建一个ej_interface_

[ ] 此部分具体实现要看 hardware_interface::JointHandle 源码 hardware_interface::EffortJointInterface 源码

=========================================================================
在这里插入图片描述
和EffortJointInterface差不多，
处理位置控制joint
注册 position joint interface ：将 joint_handle 注册到 pj_interface_ 相当于创建一个pj_interface_

[ ] 此部分具体实现要看 hardware_interface::JointHandle 源码 hardware_interface::EffortJointInterface 源码

========================================================================
在这里插入图片描述
同理和上面两个一样
注册 velocity joint interface ：将 joint_handle 注册到 vj_interface_ 相当于创建一个vj_interface_

[ ] 此部分具体实现要看 hardware_interface::JointHandle 源码 hardware_interface::EffortJointInterface 源码

=========================================================================
在这里插入图片描述
hardwareinterface必须是上面三种类型的一种，要是不是的话，报错，并返回初始化失败

=========================================================================
在这里插入图片描述
如果 hardware_interface 的标签没有加 hardware_interface/ 就建议加上

至此注册完了相应的接口

=========================================================================
在这里插入图片描述
对应gazebo的joint与robot 的joint
gazebo里的应该是urdf或xacro里声明的
robot 的joint 应该是 transmissions 里的
所以在写xacro的时候一定要对应上 urdf或xacro里声明的joint 可以不写transmissions 但是transmissions里的joint一定要在xacro里写
从 gazebo里面查找对应的 transmissions 里的 joint

gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
这行代码功能就是在 xacro文件里找到 transmissions 里joint名字的 joint ，并得到该joint得各参数赋值给 gazebo::physics::JointPtr joint

[ ] 具体怎么得到得要看gazebo::physics::ModelPtr 里得GetJoint()函数

transmissions 里的 joint 没有在在gazebo里（urdf或xacro里声明的）没有找到
返回 false 未初始化成功

=========================================================================
在这里插入图片描述
sim_joints_声明类型是 std::vector< gazebo::physics::JointPtr >
vector 的 push_back 作用就是将上面得到的joint 放入
std::vector< gazebo::physics::JointPtr > sim_joints_ 中。

后面很多的操作都是用 sim_joints_ 为对象操作的。

=========================================================================
在这里插入图片描述
// 得到gazebo得物理引擎模式为write（）提供选择（VELOCITY部分但是应该进不到这部分，在初始化函数的时候VELOCITY改为了VELOCITY_PID）一般为ode ODE(Open Dynamics Engine) gazebo 得模式可以先不管应该是 ode模式

[ ] 不确定可以打印下看看

=========================================================================
在这里插入图片描述
注册 joint 限制初始化是本类的函数具体功能到那个函数再看

=========================================================================
在这里插入图片描述
joint的控制方法是 POSITION 或者 VELOCITY 的进行pid控制，下面进行该控制器的初始化

其中

const ros::NodeHandle nh(robot_namespace + "/gazebo_ros_control/pid_gains/" + joint_names_[j]);
(pid_controllers_[j].init(nh)
进行该控制器的初始化

[ ] 具体应该看control_toolbox::Pid 的init 功能函数

并且在里面将joint_control_methods_的
POSITION改为POSITION_PID
VELOCITY改为VELOCITY_PID

是力控制 joint 的话，就设置力的上下限

=========================================================================
在这里插入图片描述
注册接口是继承来的功能函数父类RobotHW是继承于InterfaceManager的

[ ] 具体功能应该再看继承类的代码
实则是调用InterfaceManager的方法。

？？？注册完干啥了目前还不知道后面也没用到？？？

后面用的 sim_joints_ 作为 gazebo的joint接口

在这里插入图片描述

初始化紧急停止的参数

在这里插入图片描述
都完成了则初始化成功

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

DefaultRobotHWSim::initSim函数详解

DefaultRobotHWSim::initSim()

源码（加了注释）

对源码进行功能分块解读

后面用的 sim_joints_ 作为 gazebo的joint接口

初始化紧急停止的参数

热门文章

最新文章

相关电子书

热门

活动广场

任务中心

开发者评测

高校计划

乘风者计划

训练营

阿里云MVP

话题

直播

下载

镜像站

技术资料

插件

DefaultRobotHWSim::initSim函数详解

DefaultRobotHWSim::initSim()

源码 （加了注释）

对源码进行功能分块解读

后面用的 sim_joints_ 作为 gazebo的joint接口

初始化紧急停止的参数

热门文章

最新文章

相关电子书

源码（加了注释）