真空吸盘插件代码理解

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 发布消息和提供服务。

1. 设置模型指针和世界指针：

   • parent_ = _model; 将传入的模型指针保存到成员变量 parent_ 中。
   • world_ = _model->GetWorld(); 获取模型所在的世界，并保存到 world_ 成员变量中。

2. 加载机器人命名空间：

   • 从 SDF 文件中查找 robotNamespace 元素，并将其值（如果存在）添加到 robot_namespace_ 成员变量中，并在末尾添加斜杠 /。

3. 加载link名称：

   • 检查 SDF 文件中是否存在 bodyName 元素。
   • 如果存在，则获取其值并保存到 link_name_ 成员变量中。
   • 随后，使用 link_name_ 在模型中获取链接的指针，并保存到 link_ 成员变量中。
   • 如果链接不存在，则列出模型中所有link的名称，并退出函数。

4. 加载话题名称：

   • 检查 SDF 文件中是否存在 topicName 元素。
   • 如果存在，则获取其值并保存到 topic_name_ 成员变量中。
   • 如果不存在，则退出函数。

5. 初始化 ROS 节点：

   • 检查 ROS 是否已初始化。
   • 如果没有初始化，则退出函数。
   • 如果已初始化，则使用 robot_namespace_ 创建一个新的 ROS 节点句柄，并保存到 rosnode_ 成员变量中。

6. 创建 ROS 发布者：

   • 使用 ros::AdvertiseOptions 创建一个 std_msgs::Bool 类型的发布者，该发布者将使用 topic_name_ 作为其名称。
   • 发布者连接和断开时的回调函数分别是 Connect 和 Disconnect 成员函数的绑定。
   • 将发布者保存到 pub_ 成员变量中。

7. 创建 ROS 服务：

   • 创建两个服务，分别名为 “on” 和 “off”，它们分别对应 OnServiceCallback 和 OffServiceCallback 成员函数的绑定。
   • 将这两个服务保存到 srv1_ 和 srv2_ 成员变量中。

8. 启动回调队列线程：

   • 使用 boost::thread 创建一个新线程，该线程将运行 QueueThread 成员函数，以处理 ROS 回调队列中的消息。
   • 将线程保存到 callback_queue_thread_ 成员变量中。

9. 连接世界更新事件：

   • 使用 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 方法，它负责检查并更新与真空抓取器相关的物理交互。下面是对这段代码的详细解释：

1. 初始化消息:

   • 创建一个 std_msgs::Bool 类型的消息 grasping_msg，并将其 data 字段初始化为 false。这个消息稍后将用于发布到 ROS 话题上，以指示是否正在进行抓取操作。

2. 检查状态:

   • 如果 status_ 为 false（可能表示抓取器未激活或未准备好），则发布一个 grasping_msg 消息，内容为 false，然后返回。这意味着当前没有进行抓取操作。

3. 锁定互斥锁:

   • 使用 lock_ 互斥锁来确保在修改共享资源时的线程安全。

4. 获取世界姿势和模型列表:

   • 根据 Gazebo 的主版本号，使用不同的 API 调用来获取 link_（可能是抓取器链接）的世界姿势（位置和方向）以及所有模型的列表。Gazebo 8 之后的版本与之前的版本在 API 上有所不同。

5. 遍历所有模型和链接:

   • 对于世界中的每个模型，遍历其所有的链接。
   • 跳过当前 link_ 和其所属的模型（即 parent_），因为它们不需要被检查和操作。

6. 计算链接间的距离并应用力:

   • 对于每个其他链接，计算它与 link_ 之间的相对位置（diff）。
   • 如果这两个链接之间的距离（norm）小于 0.05，则认为它们“接近”或可能正在接触。
   • 在这种情况下，设置该链接的线性速度和角速度为与 link_ 相同（可能是为了模拟吸附效果）。
   • 根据两个链接之间的距离（norm）计算一个归一化的力（norm_force），并应用一个沿着它们之间方向（diff.Pos().Normalize()）的力。
   • 如果两个链接非常接近（norm < 0.01），则直接设置该链接的位置为 link_ 的位置，并保持其原始方向。
   • 限制 norm_force 的最大值为 20，以避免过大的力。
   • 如果两个链接足够接近，则将 grasping_msg.data 设置为 true，表示正在进行抓取操作。

7. 发布消息:

   • 发布 grasping_msg 到 ROS 话题上，以通知其他节点当前是否正在进行抓取操作。

8. 解锁互斥锁:

   • 释放 lock_ 互斥锁，以便其他线程可以访问共享资源。

总的来说，这个方法模拟了一个真空抓取器的行为，当抓取器与其他物体足够接近时，它会对这些物体施加一个力，并可能改变它们的位置和速度。同时，它还通过 ROS 发布一个消息来通知其他节点当前是否正在进行抓取操作。