<ROS> Gazebo Ros Control 及 Controller运用

  • 写在前面
  • Gazebo ROS Control
  • Default Robot HW SIM
  • Gazebo ROS Control Plugin
  • ROS Controller运用
    • 1 position_controllersJointPositionController
    • 2 position_controllersJointGroupPositionController
    • 3 joint_state_controller
      • 31 方式一
      • 32 方式二
      • 33 效果验证
  • 附录

1. 写在前面

第一篇 – 机器人描述: http://blog.csdn.net/sunbibei/article/details/52297524

第二篇 – TRANSMISSION: http://blog.csdn.net/sunbibei/article/details/53287975

在我上一篇博客上, 我们引入了UR5BH机器人, 另外, 对其描述文件中的transmission进行了解析, 其中摘录了如下文件:

<gazebo>
  <plugin filename="libgazebo_ros_control.so" name="ros_control">
    <!--robotNamespace>/</robotNamespace-->
    <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
  </plugin>
</gazebo>
... ...
<transmission name="shoulder_pan_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="shoulder_pan_joint">
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="shoulder_pan_motor">
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>
... ...
<transmission name="bh_j32_transmission">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="bh_j32_joint"/>
  <actuator name="bh_j32">
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
    <motorTorqueConstant>1</motorTorqueConstant>
  </actuator>
</transmission>

在这段描述中, 除了transmission之外, 另一个引入的, 就是gazebo_ros_control. 本篇作为第三篇, 主要和大家分享一些关于gazebo_ros_control相关的内容。

2. Gazebo ROS Control

在前一篇的基础上, 实际上我们已经能够开始配置MoveIt!来进行机器人控制了。 但是, 为了更进一步的让事情更加清晰, 我们还是耐下心再探讨一下gazebo_ros_control。 包布局如下图所示:

打开package.xml文件, 除了一些依赖包的定义外, 一个比较关键的地方是在export块中, 摘录如下:

... ...
<export>
    <gazebo_ros_control plugin="${prefix}/robot_hw_sim_plugins.xml"/>
</export>
... ...

了解pluginlib的同学肯定知道, 这是将plugin的定义文件导入的步骤。 不熟悉的同学, 可以参考1, 或者 参考2 进行了解。

导入的文件在文件系统中可以看到, 摘录如下:

<library path="lib/libdefault_robot_hw_sim">
  <class
    name="gazebo_ros_control/DefaultRobotHWSim"
    type="gazebo_ros_control::DefaultRobotHWSim"
    base_class_type="gazebo_ros_control::RobotHWSim">
    <description>
      A default robot simulation interface which constructs joint handles from an SDF/URDF.
    </description>
  </class>
</library>

可以看到, 该文件中定义了一个plugin, 就是gazebo_ros_control::DefaultRobotHWSim, 该plugin可以动态的被加载进来。再查看该包的CMakeLists.txt, 可以看到下述内容:

## 2.1. Libraries
add_library(${PROJECT_NAME} src/gazebo_ros_control_plugin.cpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

add_library(default_robot_hw_sim src/default_robot_hw_sim.cpp)
target_link_libraries(default_robot_hw_sim ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

该包总共会生成两个库, 第一个库是gazebo_ros_control, 第二个库是default_robot_hw_sim, 分别都只有一个文件。 下面我们分别来分析这两个库的源码以及所对应完成的任务。

3. Default Robot HW SIM

default_robot_hw_sim.cpp中, 定义了类DefaultRobotHWSim, 该类继承于gazebo_ros_control::RobotHWSim, 父类定义摘录如下:

namespace gazebo_ros_control {

  // Struct for passing loaded joint data
  struct JointData
  {
    std::string name_;
    std::string hardware_interface_;

    JointData(const std::string& name, const std::string& hardware_interface) :
      name_(name),
      hardware_interface_(hardware_interface)
    {}
  };

  class RobotHWSim : public hardware_interface::RobotHW
  {
  public:
    virtual ~RobotHWSim() { }
    virtual bool initSim(
        const std::string& robot_namespace,
        ros::NodeHandle model_nh,
        gazebo::physics::ModelPtr parent_model,
        const urdf::Model *const urdf_model, // 留意该指针有两个const
        // TransmissionInfo的内容在前一篇博客中有介绍
        std::vector<transmission_interface::TransmissionInfo> transmissions) = 0;
    virtual void readSim(ros::Time time, ros::Duration period) = 0;
    virtual void writeSim(ros::Time time, ros::Duration period) = 0;

    virtual void eStopActive(const bool active) {}
  };
}

该类定义了gazebo plugin版本的RobotHWRobotHW定义于ros control包中, 定义了与实际机器人通讯的基本接口, 在编写实际机器人的ros control体系时, 该类是用户必须实现的类。 主要重载read(), write(), preparewitch() 以及doSwitch()四个接口。 在RobotHW中, 这四个函数都有默认实现(默认实现是空, 什么都不做), 都不是纯虚函数。 一般而言, 重载读写函数即可正常工作了。 这个地方, 只是将接口换了个名字, 并且, 几个纯虚函数都必须在子类中进行实现。

DefaultRobotHWSim的定义摘录如下:

namespace gazebo_ros_control
{

class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim
{
public:

  // 四个纯需函数必须在子类中进行实现。
  virtual bool 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);
  virtual void readSim(ros::Time time, ros::Duration period);
  virtual void writeSim(ros::Time time, ros::Duration period);
  virtual void eStopActive(const bool active);

protected:
  // Methods used to control a joint. 一个关节!!!
  enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID};

  // 关节限制相关的内容, 不是我们关注的重点, 感兴趣的朋友可以参考下述网站:
  // http://wiki.ros.org/pr2_controller_manager/safety_limits
  void registerJointLimits(const std::string& joint_name,
                           const hardware_interface::JointHandle& joint_handle,
                           const ControlMethod ctrl_method,
                           const ros::NodeHandle& joint_limit_nh,
                           const urdf::Model *const urdf_model,
                           int *const joint_type, double *const lower_limit,
                           double *const upper_limit, double *const effort_limit);
  unsigned int n_dof_;
  // 关节接口, 这四个类的设计很有意思。
  hardware_interface::JointStateInterface    js_interface_;
  hardware_interface::EffortJointInterface   ej_interface_;
  hardware_interface::PositionJointInterface pj_interface_;
  hardware_interface::VelocityJointInterface vj_interface_;
  // 关节限制相关的内容
  joint_limits_interface::EffortJointSaturationInterface   ej_sat_interface_;
  joint_limits_interface::EffortJointSoftLimitsInterface   ej_limits_interface_;
  joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_;
  joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_;
  joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_;
  joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_;
  std::vector<double> joint_lower_limits_;
  std::vector<double> joint_upper_limits_;
  std::vector<double> joint_effort_limits_;

  std::vector<std::string> joint_names_;
  std::vector<int> joint_types_;
  std::vector<ControlMethod> joint_control_methods_;
  std::vector<control_toolbox::Pid> pid_controllers_;
  std::vector<double> joint_position_;
  std::vector<double> joint_velocity_;
  std::vector<double> joint_effort_;
  std::vector<double> joint_effort_command_;
  std::vector<double> joint_position_command_;
  std::vector<double> last_joint_position_command_;
  std::vector<double> joint_velocity_command_;

  std::vector<gazebo::physics::JointPtr> sim_joints_;

  // 紧急停止时, e_stop_active = true
  bool e_stop_active_, last_e_stop_active_;
};

typedef boost::shared_ptr<DefaultRobotHWSim> DefaultRobotHWSimPtr;

}

#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_

由于关于Safty Limits 并不是我们关注的内容。 所以在后面的解析过程中, 我们也会忽略该部分内容, 感兴趣的同学, 可以参考上述网站, 内容也不是很多。下面我们主要关注initSim()的实现, 其他几个函数无非是调用Gazebo的API完成关节数据的读写。

iniSim()函数的实现摘录如下:

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) {
  const ros::NodeHandle joint_limit_nh(model_nh);

  // Resize vectors to our DOF
  n_dof_ = transmissions.size(); // 也就是说, 该变量保存关节个数
  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_);

  // 初始化所有变量
  for(unsigned int j=0; j < n_dof_; j++) {
    if(transmissions[j].joints_.size() == 0) {
      ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
        << " has no associated joints.");
      continue;
    } else if(transmissions[j].joints_.size() > 1) {
      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;
    } // 判定transmission内容是否有错, DefaultRobotHWSim仅支持一个tramsmission对应一个joint

    std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].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
      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.");
    }
    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;
    } else if (joint_interfaces.size() > 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;
    } // 至少包含一个transmission, 多个的话, 将会默认使用第一个, 但并不算错误(没有continue)。

    // 初始化各变量(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
    ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint ‘" << joint_names_[j]
      << "‘ of type ‘" << hardware_interface << "‘");

    // hardware_interface::JointStateInterface初始化
    // 将类中pos, vel, eff三个变量的地址绑定对应于三个变量joint_xxx[j]
    js_interface_.registerHandle(hardware_interface::JointStateHandle(
        joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));

    // 根据申明的transmission, 创建对应的JointHandle.
    hardware_interface::JointHandle joint_handle;
    if(hardware_interface == "EffortJointInterface") {
      // 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_.registerHandle(joint_handle);
    } else if(hardware_interface == "PositionJointInterface") {
      // 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_.registerHandle(joint_handle);
    } else if(hardware_interface == "VelocityJointInterface") {
      // 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_.registerHandle(joint_handle);
    } else {
      ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for ‘"
        << hardware_interface );
      return false;
    }

    // Get the gazebo joint that corresponds to the robot joint.
    gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
    if (!joint) {
      ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j]
        << "\" which is not in the gazebo model.");
      return false;
    }
    sim_joints_.push_back(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]);
    // 初始化控制器
    if (joint_control_methods_[j] != EFFORT) {
      const ros::NodeHandle nh(model_nh, "/gazebo_ros_control/pid_gains/" +
                               joint_names_[j]);
      if (pid_controllers_[j].init(nh, true)) {
        switch (joint_control_methods_[j]) {
          case POSITION:
            joint_control_methods_[j] = POSITION_PID;
            break;
          case VELOCITY:
            joint_control_methods_[j] = VELOCITY_PID;
            break;
        }
      } else {
#if GAZEBO_MAJOR_VERSION > 2
        joint->SetParam("fmax", 0, joint_effort_limits_[j]);
#else
        joint->SetMaxForce(0, joint_effort_limits_[j]);
#endif
      }
    }
  }

  // 注册接口, 父类RobotHW是继承于InterfaceManager的
  // 实则是调用InterfaceManager的方法。
  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;
}

从上面的实现可以看到, 通过URDF文件中设定transmission, DefaultRobotHWSim解析所有的transmission块, 保存每个关节的JointHandle, 并提供读写数据的接口。其中JointHandle的管理方式是调用ROS Control中的hardware_interface, 而这一块的内容, 将在后面的博客中专门分析, 这部分的实现非常巧妙, 也很有意思。

4. Gazebo ROS Control Plugin

GazeboRosControlPlugin继承于gazebo::ModelPlugin类, 是一个可以由Gazebo加载的Plugin, 该类中最为关键的两个函数分别是: Load() and Update(). 分别的实现如下:

Load()的实现如下所示:

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;

  // 一系列的容错判断, 以及获取sdf中所定义的参数

  ... ...

  // 从sdf_中获得RobotHWSim的类型
  if(sdf_->HasElement("robotSimType")) {
    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_<<"\"");
  }

  ... ...

  // Get parameters/settings for controllers from ROS param server
  model_nh_ = ros::NodeHandle(robot_namespace_);

  ... ...

  // 从参数服务器中获得URDF
  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;
  }

  // 通过ClassLoader加载RobotHWSim(默认使用的是DefaultRobotHWSim)
  try {
    robot_hw_sim_loader_.reset
      (new pluginlib::ClassLoader<gazebo_ros_control::RobotHWSim>
        ("gazebo_ros_control",
          "gazebo_ros_control::RobotHWSim"));
    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, 在RobotHWSim中初始化所有关节句柄
    if(!robot_hw_sim_->initSim(robot_namespace_, model_nh_, parent_model_, urdf_model_ptr, transmissions_)) {
      ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface");
      return;
    }

    // 创建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_));

    // 监听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.");
}

上述函数的实现中, ... ...代表有省略部分。 仅抽离出来了比较重要的部分, 将一些容错和参数获取的代码给省略了。 可以看到, 在该函数中, 完成了RobotHW的初始化(在Gazebo中, 是通过RobotHWSim类呈现, 该类实际上也是RobotHW的子类), 完成了ControllerManager的初始化, 并将RosControl循环(update()函数)与update event进行了绑定, 在每次事件到来的时候, 都将会进行循环。

update()函数的实现如下所示:

// Called by the world update start event
void GazeboRosControlPlugin::Update() {
  // 得到当前时间以及和上一次更新的时间间隔
  gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
  ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
  ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;

  robot_hw_sim_->eStopActive(e_stop_active_);

  // Check if we should update the controllers
  if(sim_period >= control_period_) {
    // 交替更新前一次时间变量
    last_update_sim_time_ros_ = sim_time_ros;

    // 调用RobotHW接口, 读关节数据, 保证当前关节值是最新值
    robot_hw_sim_->readSim(sim_time_ros, sim_period);

    // Compute the controller commands
    bool reset_ctrlrs;
    if (e_stop_active_) {
      reset_ctrlrs = false;
      last_e_stop_active_ = true;
    } else {
      if (last_e_stop_active_) {
        reset_ctrlrs = true;
        last_e_stop_active_ = false;
      } else {
        reset_ctrlrs = false;
      }
    }
    // ControlManager类的update接口调用, 将会顺序调用每一个加载上来的控制器的update()
    controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs);
  }

  // 每一个正在运行的控制器update调用之后, 对应被控制关节的命令就更新完成
  // 现在再调用RobotHW的写操作。 完成对关节命令的写入。
  // 不但在仿真中RosControl循环是这个流程, 在实际RosControl循环中, 也是这个流程。
  robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_);
  last_write_sim_time_ros_ = sim_time_ros;// 交替更新前一次时间变量
}

5. ROS Controller运用

在前一篇中, 我们在Gazebo中已经启动起来了ur2bh机器人。 输入命令:roslaunch ur5bh_gazebo ur5bh_gazebo.launch。 在启动起来Gazebo之后, 命令行中可以看到如下几行输出:

而这几行的输出, 正是在前面Load()函数中的提示输出。 显然, 我们的GazeboRosControlPlugin已经在launch文件中, 被启动加载了。当然, 这个时候ControllerManager也处于就绪状态, 可以重新打开命令行, 输入rosservice call /controller_manager/list_controller_types进行验证, 该服务器将会返回一大堆信息。现在, 我们就可以在配置文件里面指定我们所需要的控制器了。 比如说, 由于我们在transmission块中, 指定的是PositionJointInterface, 所以, 我们可以使用的控制器只能是位置控制器。 下面我们试着加一两个控制器。

5.1. position_controllers/JointPositionController

该控制器实现于ros_controllers包中, 这个包提供了很多默认控制器, 可以直接使用, 感兴趣的同学, 也可以到Github 上进行下载查看他们的源码。要配置这些控制器, 只需要简单的配置文件即可搞定。

打开命令行, 以此输入:

roscd ur5bh_gazebo && mkdir config
vim config/controllers.yaml

将下述内容复制到该文件中:

shoulder_pan_position_controller:
  type: position_controllers/JointPositionController
  joint: shoulder_pan_joint
  pid:
    p: 100.0
    d: 10.0

第一行是控制器名称, 第二行是控制器类型, 第三行是欲控制的关节名称, 第四行是指定该控制器的PID参数。运行rosed ur5bh_gazebo ur5bh_gazebo.launch, 加入下述内容在文件中:

  <rosparam file="$(find ur5bh_gazebo)/config/controllers.yaml" command="load"/>
  <node name="joint_controller_spawner" pkg="controller_manager" type="controller_manager"
  args="spawn shoulder_pan_position_controller"
  respawn="false" output="screen"/>

上述内容, 第一行是将yaml文件中定义的内容, 加载到参数服务器。 第二行类似于在命令行中运行rosrun controller_manager controller_manager spawn shoulder_pan_position_controller. 完成的功能, 是创建一个节点, 请求ControllerManager的加载服务器的服务, 请求加载控制器shoulder_pan_position_controller, 而该控制器的相关参数定义于yaml文件中。

复制保存之后, 打开命令行, 运行roslaunch ur5bh_gazebo ur5bh_gazebo.launch, 同样, 可以看到前篇博客所示的状态, 同时, 在命令行中, 可以看到如下输出。

再打开一个命令行, 输入下述命令可以查看我们加载的控制器的效果, rostopic pub /shoulder_pan_position_controller/command std_msgs/Float64 50

在Gazebo中可以看到, 我们的机器人转动了。 我们在上述yaml中, 只是定义了一个关节的控制器, 当然, 我们可以配置多个控制器, 分别控制多个关节. 方法类似, 就不示例了。

5.2. position_controllers/JointGroupPositionController

另一种方式, 可以加载position_controllers/JointGroupPositionController, 指定一个控制器控制多个关节。示例如下, 首先在config/controllers.yaml中添加下述内容:

finger1_position_controller:
  type: position_controllers/JointGroupPositionController
  joints:
    - bh_j11_joint
    - bh_j12_joint
    - bh_j13_joint

在launch文件中, 将发起的节点那一行更换成:

  <node name="joint_controller_spawner" pkg="controller_manager" type="controller_manager"
  args="spawn shoulder_pan_position_controller finger1_position_controller"
  respawn="false" output="screen"/>

同样, 打开命令行, 运行roslaunch ur5bh_gazebo ur5bh_gazebo.launch, 打开Gazebo之后, 应该能够在命令行中看到下述输出内容:

新打开一个命令行, 输入下述命令:

rostopic pub /finger1_position_controller/command std_msgs/Float64MultiArray ‘{data: [10.0, 10.0, 10.0]}‘, 在Gazebo中, 可以看到手指动了。

5.3. joint_state_controller

一般情况下, 我们的关节数据都是需要通过/joint_states话题传递出来, 这样, RobotStatePublisher才能够解析发布/tf数据。 而现在我们是没有/joint_states话题的(可以在命令行中通过rostopic list来查看)。这个话题一个最直接的使用, 就是在rviz中。 在rviz中是可以直接查看我们机器人当前状态的。比如, 打开rviz(运行rosrun rviz rviz), 当前我们在rviz中添加RobotModel插件, 看到的应该是下述状态:

当然, 也是不能在该插件中看到我们的ur5bh机器人。 解决这个问题, ros control为我们提供了一个默认控制器, 专门用来发布/joint_states话题数据。有两种方式可以添加该控制器, 完成其中一种即可。

5.3.1. 方式一

首先在config/controllers.yaml中添加下述内容:

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

在launch文件中, 将发起的节点那一行更换成:

  <node name="joint_controller_spawner" pkg="controller_manager" type="controller_manager"
  args="spawn shoulder_pan_position_controller finger1_position_controller joint_state_controller"
  respawn="false" output="screen"/>

5.3.2. 方式二

除了通过上述方式加载joint_state_controller外, 还可以直接将该控制器写到URDF文件中去。由于我们在ur5bh_gazebo.launch文件中, 上传到参数服务器的文件是ur5bh.urdf, 所以修改该模型文件, 在ur5bh.urdf中的gazebo块中(在该文件的头部)。修改后gazebo块的内容如下(删除了其中注释的部分内容):

  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="ros_control">
    </plugin>
    <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
      <jointName>
          bh_j11_joint, bh_j12_joint, bh_j13_joint, bh_j21_joint, bh_j22_joint, bh_j23_joint,
          bh_j32_joint, bh_j33_joint, elbow_joint, shoulder_lift_joint, shoulder_pan_joint,
          wrist_1_joint, wrist_2_joint, wrist_3_joint
      </jointName>
      <updateRate>50</updateRate>
      <alwaysOn>true</alwaysOn>
    </plugin>
  </gazebo>

5.3.3. 效果验证

同样, 打开命令行, 运行roslaunch ur5bh_gazebo ur5bh_gazebo.launch, 打开Gazebo之后, 再新打开一个命令行, 输入rostopic echo /joint_states, 可以看到该话题的数据, 发布频率是按照我们指定的50Hz. 这个时候再看rviz, 其中RobotModel还是一堆错误, 这是因为该插件需要的/tf数据还是没有。 解决办法: 新打开一个命令行, 输入rosrun robot_state_publisher robot_state_publisher。 不出意外, 可以看到如下效果:

可以看到, rviz中, 模型的状态和Gazebo中是保持一直的。所以robot_state_publisher节点的运行是必要的。 可以将其直接添加到ur5bh_gazebo.launch文件中, 将下述内容添加到launch文件中:

<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" type="string" value="" />
</node>

附录

从rqt的插件中, 可以查看到整体的结构图, 可以清晰的看到/joint_states起到的作用。

下面把最终的controller.yaml 和 ur5bh_gazebo.launch 两个文件的全部内容附属在下面:

ur5bh_gazebo/config/controllers.launch

shoulder_pan_position_controller:
  type: position_controllers/JointPositionController
  joint: shoulder_pan_joint
  pid:
    p: 100.0
    d: 10.0

finger1_position_controller:
  type: position_controllers/JointGroupPositionController
  joints:
    - bh_j11_joint
    - bh_j12_joint
    - bh_j13_joint

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

ur5bh_gazebo/launch/ur5bh_gazebo.launch

<?xml version="1.0"?>
<launch>
  <!-- startup simulated world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch" />
  <!-- send robot urdf to param server -->
  <param name="robot_description"
  command="$(find xacro)/xacro.py ‘$(find ur5bh_description)/urdf/ur5bh.urdf‘" />

  <!-- push robot_description to factory and spawn robot in gazebo -->
  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
  args="-urdf -param robot_description -model robot -z 0.1"
  respawn="false" output="screen" />

  <rosparam file="$(find ur5bh_gazebo)/config/controller.yaml" command="load"/>
  <node name="joint_controller_spawner" pkg="controller_manager" type="controller_manager"
  args="spawn shoulder_pan_position_controller finger1_position_controller joint_state_controller"
  respawn="false" output="screen"/>

  <!-- Robot state publisher -->
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" type="string" value="" />
  </node>
</launch>
时间: 2024-11-09 02:17:33

<ROS> Gazebo Ros Control 及 Controller运用的相关文章

Making my own Autonomous Robot in ROS / Gazebo

Day 1: Setting up ROS: Indigo OS: Ubuntu 14.04 OS: Gazebo 7.0.0 Initialize the workspace To create the basic skeleton of the directory structure, we begin with a workspace {WORKSPACE}_ws, where we set {WORKSPACE}=mybot. cd ~ mkdir -p mybot_ws/src cd

E: Failed to fetch http://packages.ros.org/ros/ubuntu/pool/main/r/ros-kinetic-gazebo-dev/ros-kinetic-gazebo-dev_2.5.18-1xenial-20190124-142040-0800_amd64.deb 404 Not Found

参考:http://answers.ros.org/question/325039/apt-update-fails-cannot-install-pkgs-key-not-working/ 在安装一些东西时,会出现上述错误(比如,我是安装gazebo-ros包时出现)其他的有关ros的包也会出现. 原因:密钥已更新,旧密钥不再有效,导致apt update和apt install出现问题. old key: 421C365BD9FF1F717815A3895523BAEEB01FA116 ne

ROS之Gazebo

<?xml version="1.0"?> <launch> <!-- these are the arguments you can pass this launch file, for example paused:=true --> <arg name="paused" default="true"/> <arg name="use_sim_time" default

ROS机器人程序设计(原书第2版)补充资料 (柒) 第七章 3D建模与仿真 urdf Gazebo V-Rep Webots Morse

ROS机器人程序设计(原书第2版)补充资料 (柒) 第七章 3D建模与仿真 urdf Gazebo V-Rep Webots Morse 书中,大部分出现hydro的地方,直接替换为indigo或jade或kinetic,即可在对应版本中使用. 提供ROS接口的3D软件比较多,本章以最典型的Gazebo介绍为主,从Player/Stage/Gazebo发展而来,现在独立的机器人仿真开发环境,目前2016年最新版本Gazebo7.1配合ROS(kinetic)使用. 补充内容:http://blo

ROS数据可视化工具Rviz和三维物理引擎机器人仿真工具V-rep Morse Gazebo Webots USARSimRos等概述

ROS数据可视化工具Rviz和三维物理引擎机器人仿真工具V-rep Morse Gazebo Webots USARSimRos等概述 Rviz Rviz是ROS数据可视化工具,可以将类似字符串文本等枯燥的数据以二维或三维等非常形象的可视化方式进行显示,如下:   除此之外,也是重要的人机交互界面,如下以一个六足机器人为例: 单足(初始--调整):           数值结果如下: --- header: seq: 9034 stamp: secs: 1487127789 nsecs: 815

ROS,Gazebo以及Rviz的启动以及环境设置问题

在基于linux上的ros,其需要注意环境变量,如下是ros gazebo以及rviz的启动方式. rosed baxter_tools enable_robot.py 使用默认的方式打开上述的.py文件,这个打开方式是在桌面的.bashrc里面进行修改,例如可以使用export EDITOR="emacs"这是表示一个新开的窗口,也可以使用"emacs -nw"表示不是新的窗口.此外,修改的bashrc原来的窗口source .bashrc;新开的窗口就是可以的.

ROS机器人程序设计(原书第2版)补充资料 (捌) 第八章 导航功能包集入门 navigation

ROS机器人程序设计(原书第2版)补充资料 (捌) 第八章 导航功能包集入门 navigation 书中,大部分出现hydro的地方,直接替换为indigo或jade或kinetic,即可在对应版本中使用. 本章三个非常重要概念:TF,SLAM,AMCL.务必掌握. 补充内容:http://blog.csdn.net/zhangrelay/article/details/50299417 第216页: 简介本章要点. 第217页: 导航综合功能包组成架构等. 补充如下: 目录 配置并使用导航功能

Ubuntu14.04安装和配置ROS Indigo

安装ROS 配置Ubuntu的软件源 配置Ubuntu要求允许接受restricted.universe和multiverse的软件源,可以根据下面的链接配置: https://help.ubuntu.com/community/Repositories/Ubuntu 配置成如下图所示即可,一般情况下,这些配置都是默认的. 软件源配置 添加软件源到sources.list 设置软件源的代码如下: $ sudo sh -c 'echo "deb http://packages.ros.org/ro

ROS-Indigo版在Ubuntu上的安装

Ubuntu install of ROS Indigo We  are building Debian packages for several Ubuntu platforms, listed  below. These packages are more efficient than source-based builds and  are our preferred installation method for Ubuntu. If you need to install from s