机器人的ros_control硬件接口设计

最新推荐文章于 2025-04-08 23:21:59 发布

arczee

最新推荐文章于 2025-04-08 23:21:59 发布

阅读量3.8k

点赞数 8

分类专栏：机器人 C/C++ ROS

本文链接：https://blog.csdn.net/shenyan0712/article/details/103083647

版权

下面我们为一个六轴机械臂设计硬件节点。当然，你可以任意创建自己的机器人模型，原理都是一样的。其实现由MYROBOT_Hardware和MYROBOT_HardwareInterface两个类构成，在其中创建了JointStateInterface，提供关节状态信息的反馈，创建了PositionJointInterface，用于基于位置命令的控制器，包括单纯的位置命令控制器joint_group_position_controller，以及运动轨迹的控制器joint_trajectory_controller，joint_trajectory_controller便可以用于与 Moveit进行对接。

这里还只是先实现了ROS端的内容，也就是说并没有真正完成硬件的控制，但可以窥探ros_control的工作的机制。真正的硬件控制可在完成这一部分之后在进行添加。

基本代码

MYROBOT_Hardware

MYROBOT_Hardware单纯地定义了ros_control需要各个接口和变量。

class MYROBOT_Hardware : public hardware_interface::RobotHW
{
protected:
  hardware_interface::JointStateInterface       jnt_state_interface_;
  hardware_interface::PositionJointInterface    jnt_pos_interface_;
  hardware_interface::VelocityJointInterface    jnt_vel_interface_;
  hardware_interface::EffortJointInterface      jnt_eff_interface_;

  joint_limits_interface::PositionJointSaturationInterface  jnt_pos_saturation_interface_;
  joint_limits_interface::PositionJointSoftLimitsInterface  jnt_pos_limits_interface_;
  //... and joint saturation and limits for position & velocity

  int                       num_jnts_;
  std::vector<std::string>  jnt_names_;
  std::vector<int>          jnt_type_;
  std::vector<double>       jnt_cmd_;
  std::vector<double>       jnt_pos_;
  std::vector<double>       jnt_vel_;
  std::vector<double>       jnt_eff_;
  std::vector<double>       jnt_lower_limits_;
  std::vector<double>       jnt_upper_limits_;
}

MYROBOT_HardwareInterface类定义

class MYROBOT_HardwareInterface: public MYROBOT_Hardware
{
  public:
    MYROBOT_HardwareInterface(ros::NodeHandle& nh);
    ~MYROBOT_HardwareInterface();
    void init();
    void update(const ros::TimerEvent& e);
    void read();
    void write(ros::Duration elapsed_time);
  protected:
    ros::NodeHandle nh_;  //
    ros::Timer timer_;  //定期更新的定时器
    double control_period_;  //控制周期
    ros::Duration elapsed_time_;
    boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
}

构造函数

MYROBOT_HardwareInterface::MYROBOT_HardwareInterface(ros::NodeHandle& nh) \
		: nh_(nh)
{
  //1. 调用init完成接口的注册
  init();
  //2. 创建
  controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
  nh_.param("/myrobot/hardware_interface/control_period", control_period_, 0.1);
  //3. 创建用于定期更新的定时器
  non_realtime_loop_ = nh_.createTimer(ros::Duration(control_period_), &MYROBOT_HardwareInterface::update, this);
}

第2步中从"/myrobot/hardware_interface/control_period"读取更新周期。关于硬件的配置都放在如下的hardware.yaml文件中，由launch文件进行加载。

hrt120:
  hardware_interface:
     control_period: 0.01 # second
     joints:
       - joint_1
       - joint_2
       - joint_3
       - joint_4
       - joint_5
       - joint_6

init()函数

void control_period_HardwareInterface::init()
{
  //1. 取得关节名
  nh_.getParam("/myrobot/hardware_interface/joints", joint_names_);
  if (joint_names_.size() == 0)
  {
    ROS_FATAL_STREAM_NAMED("init", "No joints found on '/myrobot/hardware_interface/joints'");
  }
  num_joints_ = joint_names_.size();

  //2. 按指定的关节数重新分配空间
  num_jnts_ = jnt_names_.size();
  jnt_cmd_.resize(num_jnts_);
  jnt_pos_.resize(num_jnts_);
  jnt_vel_.resize(num_jnts_);
  jnt_eff_.resize(num_jnts_);

  //3. 初始化各关节对应的各种接口
  for(int i= 0; i<num_jnts_; i++) {
    cout<<"\t"<<jnt_names_[i]<<endl;
    // 状态Handle
    Join