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.
=========================================================================================================
扫一扫
9万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
