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 绑定对应得状态（三个：positionelocity、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 绑定对应得状态（三个：positionelocity、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接口
初始化紧急停止的参数
都完成了则初始化成功
======================================================================
