joint_trajectory_controller(二)read_state_from_state_interfaces详解

读取硬件状态:

void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state)
{
  auto assign_point_from_interface =
    [&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
  {
    for (size_t index = 0; index < dof_; ++index)
    {
      trajectory_point_interface[index] = joint_interface[index].get().get_value();
    }
  };

  // Assign values from the hardware
  // Position states always exist
  assign_point_from_interface(state.positions, joint_state_interface_[0]);
  // velocity and acceleration states are optional
  if (has_velocity_state_interface_)
  {
    assign_point_from_interface(state.velocities, joint_state_interface_[1]);
    // Acceleration is used only in combination with velocity
    if (has_acceleration_state_interface_)
    {
      assign_point_from_interface(state.accelerations, joint_state_interface_[2]);
    }
    else
    {
      // Make empty so the property is ignored during interpolation
      state.accelerations.clear();
    }
  }
  else
  {
    // Make empty so the property is ignored during interpolation
    state.velocities.clear();
    state.accelerations.clear();
  }
}

 使用到了assign_point_from_interface函数查看该函数的定义：

定义了一个函数,将每个自由度的值赋值给另一个变量来存放;

从joint_state_interface_[0]中获取位置信息,从另外的内容中分别获取速度和加速度信息.因此搜索joint_state_interface_,查到了是从这个方法中获取当前接口位置:

for (const auto & interface : params_.state_interfaces)
  {
    auto it =
      std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
    auto index = std::distance(allowed_interface_types_.begin(), it);
    if (!controller_interface::get_ordered_interfaces(
          state_interfaces_, params_.joints, interface, joint_state_interface_[index]))
    {
      RCLCPP_ERROR(
        logger, "Expected %zu '%s' state interfaces, got %zu.", dof_, interface.c_str(),
        joint_state_interface_[index].size());
      return CallbackReturn::ERROR;
    }
  }

 所以搜索get_ordered_interfaces,发现本地搜不到,应该是没下载到源码,在github中的roscontrol上搜索,找到定义:

bool get_ordered_interfaces(
  std::vector<T> & unordered_interfaces, const std::vector<std::string> & ordered_names,
  const std::string & interface_type, std::vector<std::reference_wrapper<T>> & ordered_interfaces)
{
  ordered_interfaces.reserve(ordered_names.size());
  for (const auto & name : ordered_names)
  {
    for (auto & interface : unordered_interfaces)
    {
      if (!interface_type.empty())
      {
        // check case where:
        // (<joint> == <joint> AND <interface> == <interface>) OR <joint>/<interface> == 'full name'
        if (
          ((name == interface.get_prefix_name()) &&
           (interface_type == interface.get_interface_name())) ||
          ((name + "/" + interface_type) == interface.get_name()))
        {
          ordered_interfaces.push_back(std::ref(interface));
        }
      }
      else
      {
        if (name == interface.get_name())
        {
          ordered_interfaces.push_back(std::ref(interface));
        }
      }
    }
  }

  return ordered_names.size() == ordered_interfaces.size();
}

为输出的位置信息预先分配内存:

 ordered_interfaces.reserve(ordered_names.size());

该函数从state_interfaces_中获取关节状态信息.遍历每一项:

for (const auto & name : ordered_names)

将关节点击状态以引用的方式添加到容器中,确保状态发生改变时,引用同时改变:

ordered_interfaces.push_back(std::ref(interface));

state_interfaces_定义如下所示,该代码在gazebo_ros2_control/gazebo_ros2_control/src /gazebo_system.cpp中,是gazebo和ros之间的接口.

std::vector<hardware_interface::StateInterface> state_interfaces_;

因此,该方法主要是获得硬件状态.而变量state_interfaces_为gazebo的接口,state_interfaces_其中包含了位置速度加速度力矩等信息,因此后面考虑真实机器人时,则需要考虑真实机器人接口的方式.

gazebo的代码如下所示:

this->dataPtr->state_interfaces_.emplace_back(
          joint_name,
          hardware_interface::HW_IF_POSITION,
          &this->dataPtr->joint_position_[j]);
        initial_position = get_initial_value(joint_info.state_interfaces[i]);
        this->dataPtr->joint_position_[j] = initial_position;

该代码将指针指向的内容引用,添加到state_interfaces_数组中,并通过joint_info.state_interfaces获得初始位置.并将初始位置存放到指针指向的内容中.

从state_interfaces_中提取硬件接口中的信息,将其分别赋值给joint_state_interface_[index]中

至于joint_info的来历,可以再往后推,

auto & joint_info = hardware_info.joints[j];

void GazeboSystem::registerJoints(
  const hardware_interface::HardwareInfo & hardware_info,
  gazebo::physics::ModelPtr parent_model)

registerJoints(hardware_info, parent_model);

const hardware_interface::HardwareInfo & hardware_info,
  sdf::ElementPtr sdf)

 在registerJoints中,有仿真机械臂状态与控制动态同步的内容,如下:

for (const auto & mimic_joint : hardware_info.mimic_joints) {
    this->dataPtr->sim_joints_[mimic_joint.joint_index]->SetPosition(
      0,
      mimic_joint.offset + (mimic_joint.multiplier *
      this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index]),
      true);
  }

hardware_info的定义文件:

#include "hardware_interface/hardware_info.hpp"

struct HardwareInfo
{
  /// Name of the hardware.
  std::string name;
  /// Type of the hardware: actuator, sensor or system.
  std::string type;
  ///  Hardware group to which the hardware belongs.
  std::string group;
  /// Component is async
  bool is_async;
  /// Name of the pluginlib plugin of the hardware that will be loaded.
  std::string hardware_plugin_name;
  /// (Optional) Key-value pairs for hardware parameters.
  std::unordered_map<std::string, std::string> hardware_parameters;
  /**
   * Vector of joints provided by the hardware.
   * Required for Actuator and System Hardware.
   */
  std::vector<ComponentInfo> joints;
  /**
   * Vector of mimic joints.
   */
  std::vector<MimicJoint> mimic_joints;
  /**
   * Vector of sensors provided by the hardware.
   * Required for Sensor and optional for System Hardware.
   */
  std::vector<ComponentInfo> sensors;
  /**
   * Vector of GPIOs provided by the hardware.
   * Optional for any hardware components.
   */
  std::vector<ComponentInfo> gpios;
  /**
   * Vector of transmissions to calculate ratio between joints and physical actuators.
   * Optional for Actuator and System Hardware.
   */
  std::vector<TransmissionInfo> transmissions;
  /**
   * The XML contents prior to parsing
   */
  std::string original_xml;
  /**
   * The URDF parsed limits of the hardware components joint command interfaces
   */
  std::unordered_map<std::string, joint_limits::JointLimits> limits;

  /**
   * Map of software joint limits used for clamping the command where the key is the joint name.
   * Optional If not specified or less restrictive than the JointLimits uses the previous
   * JointLimits.
   */
  std::unordered_map<std::string, joint_limits::SoftJointLimits> soft_limits;
};

}  // namespace hardware_interface

joint_trajectory_controller中释放接口的代码:

for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
  {
    joint_command_interface_[index].clear();
    joint_state_interface_[index].clear();
  }
  release_interfaces();