void GazeboRosVacuumGripper::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
parent_ = _model;
world_ = _model->GetWorld();
robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace"))
robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
if (!_sdf->HasElement("bodyName"))
{
return;
}
else
link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
link_ = _model->GetLink(link_name_);
if (!link_)
{
std::string found;
physics::Link_V links = _model->GetLinks();
for (size_t i = 0; i < links.size(); i++) {
found += std::string(" ") + links[i]->GetName();
}
return;
}
if (!_sdf->HasElement("topicName"))
{
return;
}
else
topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
if (!ros::isInitialized())
{
return;
}
rosnode_ = new ros::NodeHandle(robot_namespace_);
ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<std_msgs::Bool>(topic_name_, 1, boost::bind(&GazeboRosVacuumGripper::Connect, this), boost::bind(&GazeboRosVacuumGripper::Disconnect, this), ros::VoidPtr(), &queue_);
pub_ = rosnode_->advertise(ao);
ros::AdvertiseServiceOptions aso1 = ros::AdvertiseServiceOptions::create<std_srvs::Empty>("on", boost::bind(&GazeboRosVacuumGripper::OnServiceCallback, this, _1, _2), ros::VoidPtr(), &queue_);
srv1_ = rosnode_->advertiseService(aso1);
ros::AdvertiseServiceOptions aso2 = ros::AdvertiseServiceOptions::create<std_srvs::Empty>("off", boost::bind(&GazeboRosVacuumGripper::OffServiceCallback, this, _1, _2), ros::VoidPtr(), &queue_);
srv2_ = rosnode_->advertiseService(aso2);
callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosVacuumGripper::QueueThread,this ) );
update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosVacuumGripper::UpdateChild, this));
}
好的,我会为您解释这段代码 GazeboRosVacuumGripper::Load
方法的内容。
这个方法主要用于加载和初始化 GazeboRosVacuumGripper
插件,使其能够与 Gazebo 物理模型交互,并通过 ROS 发布消息和提供服务。
-
设置模型指针和世界指针:
parent_ = _model;
将传入的模型指针保存到成员变量parent_
中。world_ = _model->GetWorld();
获取模型所在的世界,并保存到world_
成员变量中。
-
加载机器人命名空间:
- 从 SDF 文件中查找
robotNamespace
元素,并将其值(如果存在)添加到robot_namespace_
成员变量中,并在末尾添加斜杠/
。
- 从 SDF 文件中查找
-
加载link名称:
- 检查 SDF 文件中是否存在
bodyName
元素。 - 如果存在,则获取其值并保存到
link_name_
成员变量中。 - 随后,使用
link_name_
在模型中获取链接的指针,并保存到link_
成员变量中。 - 如果链接不存在,则列出模型中所有link的名称,并退出函数。
- 检查 SDF 文件中是否存在
-
加载话题名称:
- 检查 SDF 文件中是否存在
topicName
元素。 - 如果存在,则获取其值并保存到
topic_name_
成员变量中。 - 如果不存在,则退出函数。
- 检查 SDF 文件中是否存在
-
初始化 ROS 节点:
- 检查 ROS 是否已初始化。
- 如果没有初始化,则退出函数。
- 如果已初始化,则使用
robot_namespace_
创建一个新的 ROS 节点句柄,并保存到rosnode_
成员变量中。
-
创建 ROS 发布者:
- 使用
ros::AdvertiseOptions
创建一个std_msgs::Bool
类型的发布者,该发布者将使用topic_name_
作为其名称。 - 发布者连接和断开时的回调函数分别是
Connect
和Disconnect
成员函数的绑定。 - 将发布者保存到
pub_
成员变量中。
- 使用
-
创建 ROS 服务:
- 创建两个服务,分别名为 “on” 和 “off”,它们分别对应
OnServiceCallback
和OffServiceCallback
成员函数的绑定。 - 将这两个服务保存到
srv1_
和srv2_
成员变量中。
- 创建两个服务,分别名为 “on” 和 “off”,它们分别对应
-
启动回调队列线程:
- 使用
boost::thread
创建一个新线程,该线程将运行QueueThread
成员函数,以处理 ROS 回调队列中的消息。 - 将线程保存到
callback_queue_thread_
成员变量中。
- 使用
-
连接世界更新事件:
- 使用
event::Events::ConnectWorldUpdateBegin
连接到 Gazebo 的世界更新开始事件。 - 当世界开始更新时,将调用
UpdateChild
成员函数。 - 将此连接保存到
update_connection_
成员变量中。
- 使用
总的来说,这个 Load
方法主要用于初始化插件,设置与模型、世界和 ROS 的连接,以及创建发布者和服务,以便在运行时与 Gazebo 和 ROS 进行交互。
void GazeboRosVacuumGripper::UpdateChild()
{
std_msgs::Bool grasping_msg;
grasping_msg.data = false;
if (!status_) {
pub_.publish(grasping_msg);
return;
}
lock_.lock();
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Pose3d parent_pose = link_->WorldPose();
physics::Model_V models = world_->Models();
#else
ignition::math::Pose3d parent_pose = link_->GetWorldPose().Ign();
physics::Model_V models = world_->GetModels();
#endif
for (size_t i = 0; i < models.size(); i++) {
if (models[i]->GetName() == link_->GetName() ||
models[i]->GetName() == parent_->GetName())
{
continue;
}
physics::Link_V links = models[i]->GetLinks();
for (size_t j = 0; j < links.size(); j++) {
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Pose3d link_pose = links[j]->WorldPose();
#else
ignition::math::Pose3d link_pose = links[j]->GetWorldPose().Ign();
#endif
ignition::math::Pose3d diff = parent_pose - link_pose;
double norm = diff.Pos().Length();
if (norm < 0.05) {
#if GAZEBO_MAJOR_VERSION >= 8
links[j]->SetLinearVel(link_->WorldLinearVel());
links[j]->SetAngularVel(link_->WorldAngularVel());
#else
links[j]->SetLinearVel(link_->GetWorldLinearVel());
links[j]->SetAngularVel(link_->GetWorldAngularVel());
#endif
double norm_force = 1 / norm;
if (norm < 0.01) {
link_pose.Set(parent_pose.Pos(), link_pose.Rot());
links[j]->SetWorldPose(link_pose);
}
if (norm_force > 20) {
norm_force = 20;
}
ignition::math::Vector3d force = norm_force * diff.Pos().Normalize();
links[j]->AddForce(force);
grasping_msg.data = true;
}
}
}
pub_.publish(grasping_msg);
lock_.unlock();
}
这段代码是 GazeboRosVacuumGripper
类中的 UpdateChild
方法,它负责检查并更新与真空抓取器相关的物理交互。下面是对这段代码的详细解释:
-
初始化消息:
- 创建一个
std_msgs::Bool
类型的消息grasping_msg
,并将其data
字段初始化为false
。这个消息稍后将用于发布到 ROS 话题上,以指示是否正在进行抓取操作。
- 创建一个
-
检查状态:
- 如果
status_
为false
(可能表示抓取器未激活或未准备好),则发布一个grasping_msg
消息,内容为false
,然后返回。这意味着当前没有进行抓取操作。
- 如果
-
锁定互斥锁:
- 使用
lock_
互斥锁来确保在修改共享资源时的线程安全。
- 使用
-
获取世界姿势和模型列表:
- 根据 Gazebo 的主版本号,使用不同的 API 调用来获取
link_
(可能是抓取器链接)的世界姿势(位置和方向)以及所有模型的列表。Gazebo 8 之后的版本与之前的版本在 API 上有所不同。
- 根据 Gazebo 的主版本号,使用不同的 API 调用来获取
-
遍历所有模型和链接:
- 对于世界中的每个模型,遍历其所有的链接。
- 跳过当前
link_
和其所属的模型(即parent_
),因为它们不需要被检查和操作。
-
计算链接间的距离并应用力:
- 对于每个其他链接,计算它与
link_
之间的相对位置(diff
)。 - 如果这两个链接之间的距离(
norm
)小于 0.05,则认为它们“接近”或可能正在接触。 - 在这种情况下,设置该链接的线性速度和角速度为与
link_
相同(可能是为了模拟吸附效果)。 - 根据两个链接之间的距离(
norm
)计算一个归一化的力(norm_force
),并应用一个沿着它们之间方向(diff.Pos().Normalize()
)的力。 - 如果两个链接非常接近(
norm < 0.01
),则直接设置该链接的位置为link_
的位置,并保持其原始方向。 - 限制
norm_force
的最大值为 20,以避免过大的力。 - 如果两个链接足够接近,则将
grasping_msg.data
设置为true
,表示正在进行抓取操作。
- 对于每个其他链接,计算它与
-
发布消息:
- 发布
grasping_msg
到 ROS 话题上,以通知其他节点当前是否正在进行抓取操作。
- 发布
-
解锁互斥锁:
- 释放
lock_
互斥锁,以便其他线程可以访问共享资源。
- 释放
总的来说,这个方法模拟了一个真空抓取器的行为,当抓取器与其他物体足够接近时,它会对这些物体施加一个力,并可能改变它们的位置和速度。同时,它还通过 ROS 发布一个消息来通知其他节点当前是否正在进行抓取操作。