ros_control從入門到放棄：寫一個機械臂的硬件接口，並與moveit進行對接控制（附機械臂例子）

在創建自己的機械臂的時候需要一個動作服務器controller和RobotHW與moveit進行對接，如下圖，而幸運的是這些都已經被寫好了框架，下面就來看看原理與實現吧。
參考：
http://wiki.ros.org/ros_control
https://github.com/ros-controls/ros_control/wiki
hardware_interface::RobotHW Class Reference

先上幾張圖有下整體印象。

下面是一些常用的控制器接口。

一個最簡單的控制環就如下圖這樣：讀取硬件狀態－控制器管理器更新－寫命令到硬件

下面開始寫一個標準接口。
假設我們有一個帶有2個關節的機器人：關節A和B。兩個關節都是位置控制模式。 在這種情況下，你的機器人應該提供標準的JointPositionInterface和JointStateInterface，這樣它就可以重用所有已編寫的控制器來使用JointPositionInterface和JointStateInterface。
MyRobot類:

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class MyRobot : public hardware_interface::RobotHW//繼承hardware_interface::RobotHW類
{
public:
  MyRobot() 
 { 
   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_a("A", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_a);//讀狀態

   hardware_interface::JointStateHandle state_handle_b("B", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_b);

   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("A"), &cmd[0]);//寫命令
   jnt_pos_interface.registerHandle(pos_handle_a);

   hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("B"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_b);

   registerInterface(&jnt_pos_interface);
  }

private:
  hardware_interface::JointStateInterface jnt_state_interface;
  hardware_interface::PositionJointInterface jnt_pos_interface;
  double cmd[2];
  double pos[2];
  double vel[2];
  double eff[2];
};

那麼這段代碼如何實際控制你的機器人呢？

• 上述功能旨在使控制器管理器controller manager（以及控制器管理器內的控制器）能夠訪問機器人的關節狀態以及機器人的命令。 當控制器管理器運行時，控制器將讀取機器人中的pos，vel和eff變量，並將所需的命令寫入cmd變量。
• 確保pos，vel和eff變量始終具有最新的關節狀態joint states是你的工作，你還需要確保寫入cmd變量的內容由機器人執行。 爲此，你可以向機器人類添加read()和write()函數。 在你的main()中可以這樣寫：

main()
{
  MyRobot robot;
  controller_manager::ControllerManager cm(&robot);

  while (true)
  {
     robot.read();
     cm.update(robot.get_time(), robot.get_period());
     robot.write();
     sleep();
  }
}

如上圖所示，我們不僅限於從一個單一接口繼承。 機器人可以提供任意數量的接口。 例如，你的機器人可以提供PositionJointInterface和VelocityJointInterface等等。

舉個例子

寫一個控制器，通過roslaunch啓動。先看效果，後面上全套代碼，可以直接複製粘貼使用。
可以看到話題如下：

可以通過rqt中的Joint trajectory controller控制

通過配置moveit中的controllers.yaml保持話題名一致來實現moveit對控制器的控制：

controller_list:
  - name: /dh_arm_controller 
    action_ns: "follow_joint_trajectory"
    type: FollowJointTrajectory
    default: true
    joints:
      - arm_joint_1
      - arm_joint_2
      - arm_joint_3
      - arm_joint_4


joints:
      - arm_joint_1
      - arm_joint_2
      - arm_joint_3
      - arm_joint_4

規劃運行時發送的數據效果：

• 文件樹：
  ├── CMakeLists.txt
  ├── config
  │ └── dh_arm_control.yaml
  ├── include
  │ └── dhrobot_controller
  │ └── arm_hardware_interface.h
  ├── launch
  │ └── arm_ros_control.launch
  ├── package.xml
  └── src
  └── arm_hardware_interface.cpp

• dh_arm_control.yaml

dh_arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
     - arm_joint_1
     - arm_joint_2
     - arm_joint_3
     - arm_joint_4

  constraints:
      goal_time: 0.5
      stopped_velocity_tolerance: 0.2
  stop_trajectory_duration: 0
  state_publish_rate:  20
  action_monitor_rate: 10

• arm_hardware_interface.h

#ifndef ARM_HW_INTERFACE
#define ARM_HW_INTERFACE

#include <ros/ros.h>
#include <urdf/model.h>
#include <pthread.h>
#include <time.h>

#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>
#include <dynamixel_msgs/JointState.h>

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

#include <controller_manager/controller_manager.h>
#include <controller_manager_msgs/ListControllers.h>

class ArmHWInterface : public hardware_interface::RobotHW
{
public:
    ArmHWInterface();
    void read(const dynamixel_msgs::JointStateConstPtr &msg);
    void publishCmd();
    ros::NodeHandle getnode();

private:
    hardware_interface::JointStateInterface jnt_state_interface;
    hardware_interface::PositionJointInterface jnt_cmd_interface;
    double cmd[5];
    double pos[5];
    double vel[5];
    double eff[5];

    controller_manager_msgs::ListControllersRequest list_req;
    controller_manager_msgs::ListControllersResponse list_resp;

    bool loaded_flag;

    ros::NodeHandle _n;
    ros::Publisher pub_cmd[5];
    std_msgs::Float64 cmd_msg[5];
    ros::Time start_time_;
    ros::Duration start_dur_;

    ros::Subscriber sub_js[5];
    ros::ServiceClient client_controller_list;

};

#endif

• arm_ros_control.launch

<launch>    
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find dhrobot_description)/urdf/dhrobot.urdf.xacro'" />

  <node name="arm_hardware_interface" pkg="dhrobot_controller" type="arm_hardware_interface" output="screen"/>
  
  <rosparam file="$(find dhrobot_controller)/config/dh_arm_control.yaml" command="load"/>
  <node name="dh_arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn dh_arm_controller" respawn="false" output="screen"/>
</launch>

• arm_hardware_interface.cpp

#include "dhrobot_controller/arm_hardware_interface.h"

boost::mutex Arm_io_mutex;

ArmHWInterface::ArmHWInterface()
{
    for(int i=0; i<4; i++)
    {
        std::string joint_cmd_name="arm_joint_";
        std::string joint_state_name="arm_joint_";
        std::string joint_num=boost::lexical_cast<std::string>(i+1);
        joint_cmd_name.append(joint_num);
        joint_state_name.append(joint_num);
        joint_cmd_name.append("_controller/command");
        joint_state_name.append("_controller/state");
        pub_cmd[i]=_n.advertise<std_msgs::Float64>(joint_cmd_name, 1);
        sub_js[i]=_n.subscribe(joint_state_name, 1, &ArmHWInterface::read, this);
        client_controller_list=_n.serviceClient<controller_manager_msgs::ListControllers>("/controller_manager/list_controllers");
        loaded_flag=false;
    }

    for(int i=0; i<4; i++)
    {
        std::string joint_name="arm_joint_";
        std::string joint_num=boost::lexical_cast<std::string>(i+1);
        joint_name.append(joint_num);
        hardware_interface::JointStateHandle jnt_state_handle_tmp(joint_name, &pos[i], &vel[i], &eff[i]);
        jnt_state_interface.registerHandle(jnt_state_handle_tmp);
    }

    registerInterface(&jnt_state_interface);

    for(int i=0; i<4; i++)
    {
        std::string joint_name="arm_joint_";
        std::string joint_num=boost::lexical_cast<std::string>(i+1);
        joint_name.append(joint_num);
        hardware_interface::JointHandle jnt_handle_tmp(jnt_state_interface.getHandle(joint_name), &cmd[i]);
        jnt_cmd_interface.registerHandle(jnt_handle_tmp);
    }

    registerInterface(&jnt_cmd_interface);

    start_time_=ros::Time::now();
    start_dur_.operator +=(ros::Duration(3));
}

void ArmHWInterface::publishCmd()
{
    if(ros::Time::now()-start_time_<start_dur_)
        return;
    for(int i=0; i<4; i++)
    {
        cmd_msg[i].data=cmd[i];
        pub_cmd[i].publish(cmd_msg[i]);
    }
}

void ArmHWInterface::read(const dynamixel_msgs::JointStateConstPtr &msg)
{
    if(msg->motor_ids.size()<=0)
    {
        return;
    }
    if(msg->motor_ids[0]>6 || msg->motor_ids[0]<0)
    {
        return;
    }
    int msg_num=msg->motor_ids[0];
    double bm=msg->current_pos-pos[msg_num];
    boost::mutex::scoped_lock lock(Arm_io_mutex);
    pos[msg_num]=msg->current_pos;
    if(ros::Time::now()-start_time_>start_dur_)
    {
        if(bm>=0)
            vel[msg_num]=msg->velocity;
        else
            vel[msg_num]=-1*msg->velocity;
    }
    else
        vel[msg_num]=0;

    if(fabs(vel[msg_num])<1.2)
        vel[msg_num]=0;

    eff[msg_num]=msg->load;
}

ros::NodeHandle ArmHWInterface::getnode()
{
    return _n;
}

static void timespecInc(struct timespec &tick, int nsec)
{
  int SEC_2_NSEC = 1e+9;
  tick.tv_nsec += nsec;
  while (tick.tv_nsec >= SEC_2_NSEC)
  {
    tick.tv_nsec -= SEC_2_NSEC;
    ++tick.tv_sec;
  }
}

void* update_loop(void* threadarg)
{
    controller_manager::ControllerManager *c=(controller_manager::ControllerManager *)threadarg;
    ros::Rate r(50);
    ros::Duration d(0.02);

    while(ros::ok())
    {
        boost::mutex::scoped_lock lock(Arm_io_mutex);
        c->update(ros::Time::now(), d);
        lock.unlock();
        r.sleep();
    }
}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"Dhrobot Arm_hardware_interface", ros::init_options::AnonymousName);
    ArmHWInterface c1;


    controller_manager::ControllerManager cm(&c1);
    pthread_t tid;
    pthread_create(&tid, NULL, update_loop, (void *)&cm);


    ROS_INFO("Arm bring up successfully");
    // loop
    ros::Rate r(50);
    while(ros::ok())
    {
        boost::mutex::scoped_lock lock(Arm_io_mutex);
        c1.publishCmd();
        lock.unlock();
        ros::spinOnce();
        r.sleep();
    }
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(dhrobot_controller)


find_package(catkin REQUIRED COMPONENTS
  actionlib
  urdf
  cmake_modules
  control_msgs
  control_toolbox
  controller_manager
  hardware_interface
  joint_limits_interface
  roscpp
  sensor_msgs
  std_msgs
  trajectory_msgs
  transmission_interface
)

catkin_package(


)

include_directories(
  include/
  ${catkin_INCLUDE_DIRS}
)

add_executable(arm_hardware_interface src/arm_hardware_interface.cpp)
target_link_libraries(arm_hardware_interface ${catkin_LIBRARIES})
                      

package.xml

<?xml version="1.0"?>
<package>
  <name>dhrobot_controller</name>
  <version>1.0.0</version>
  <description>The dhrobot_controller package</description>


  <maintainer email="[email protected]">wxw</maintainer>
  <license>BSD</license>



  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>actionlib</build_depend>
  <build_depend>urdf</build_depend>
  <build_depend>cmake_modules</build_depend>
  <build_depend>control_msgs</build_depend>
  <build_depend>control_toolbox</build_depend>
  <build_depend>controller_manager</build_depend>
  <build_depend>hardware_interface</build_depend>
  <build_depend>joint_limits_interface</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>trajectory_msgs</build_depend>
  <build_depend>transmission_interface</build_depend>
  <run_depend>actionlib</run_depend>
  <run_depend>urdf</run_depend>
  <run_depend>cmake_modules</run_depend>
  <run_depend>control_msgs</run_depend>
  <run_depend>control_toolbox</run_depend>
  <run_depend>controller_manager</run_depend>
  <run_depend>hardware_interface</run_depend>
  <run_depend>joint_limits_interface</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>trajectory_msgs</run_depend>
  <run_depend>transmission_interface</run_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>