用ROS控制KUKA youbot 的5自由度机械臂和夹子

1. 修改配置文件

    在这里首先要说明的是，youbot的机械臂不需要底座也可以用电脑来控制。也就是说机械臂的驱动和控制都在那个机械臂内，另外一点，它的eth0和eth1的口是不能同时插在两个电脑上的，即使说是同时插上，但是只有一个电脑进行控制，这样的话，也是控制不了的，在实验和小伙伴一起试过。

    还有一点要注意，修改config文件，因为在有底座和手臂的情况下，我们的Joint编号是从底座开始的，也就是Joint1，Joint2，Joint3，Joint4对应于四个万向轮，Joint5-Joint9对应于5自由度的机械臂。如果单独使用机械臂，这里的youbot-manipulator.cfg文件的joint号要由Joint5-Joint9改为Joint1-Joint5才可以识别。

文件在/opt/ros/hydro/share/youbot_driver/config目录下，修改youbot-manipulator.cfg

在文件夹/opt/ros/hydro/share/youbot_driver_ros_interface/launch

将youbot_driver.launch这句的value值由true改为false，我们没有base，单独控制机械臂。

修改完以后就可以用命令加载youbot的驱动了

roslaunch youbot_driver_ros_interface youbot_driver.launch

合并action相关的topic后的节点图如下所示：

2. 用node生成特定的消息给youbot_driver 的topic

    由上面的节点关系图，我们可以看出youbot_driver已经为我们加载了一些topic，尤其要注意的是它订阅的topic，也就是上图中左边的前四个。另外，这里的driver也只有位置控制和速度控制，并没有力矩控制。前两个topic我们可以看出是arm和gripper（夹子）的位置控制，第三个是arm的速度控制，最后一个是action控制。本篇文章想要介绍的是前两个topic的控制。

下面是控制代码，官方给出的是fuerte的版本，因为太久没更新，有些地方要做修改，下面是已经修改完毕的代码。包括cpp，CMakeList和xml文件。
xml文件

<?xml version="1.0"?>
<package>
  <name>youbot_ros_simple_trajectory</name>
  <version>0.0.0</version>
  <description>The youbot_ros_simple_trajectory package</description>

  <maintainer email="yake@todo.todo">yake</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>actionlib</build_depend>
  <build_depend>control_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>trajectory_msgs</build_depend>
  <build_depend>brics_actuator</build_depend>
  
  <run_depend>actionlib</run_depend>
  <run_depend>control_msgs</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>trajectory_msgs</run_depend>
  <run_depend>brics_actuator</run_depend>


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

  </export>
</package>

CMakeList文件

cmake_minimum_required(VERSION 2.8.3)
project(youbot_ros_simple_trajectory)


find_package(catkin REQUIRED COMPONENTS
  actionlib
  control_msgs
  roscpp
  rospy
  trajectory_msgs
  brics_actuator
)


## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
  LIBRARIES youbot_ros_simple_trajectory
  CATKIN_DEPENDS actionlib control_msgs roscpp rospy trajectory_msgs brics_actuator
  DEPENDS system_lib
)


include_directories(
  ${catkin_INCLUDE_DIRS}
)


add_executable(youbot_ros_p2p src/youbot_ros_simple_trajectory.cpp)

add_executable(youbot_ctl src/youbot_trajectory_controller.cpp)


 target_link_libraries(youbot_ros_p2p
   ${catkin_LIBRARIES}
 )
 
 target_link_libraries(youbot_ctl
   ${catkin_LIBRARIES}
 )

Cpp文件

#include <iostream>
#include <assert.h>

#include <boost/units/io.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/conversion.hpp>

#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/plane_angle.hpp>

#include "ros/ros.h"
#include "trajectory_msgs/JointTrajectory.h"
#include "brics_actuator/CartesianWrench.h"
#include "brics_actuator/JointPositions.h"
#include <control_msgs/FollowJointTrajectoryActionGoal.h>

using namespace std;

//static const double INIT_POS[] = {2.94961, 1.352, -2.591, 0.1, 0.12}; // Twist fold
//static const double INIT_POS[] = {2.56, 1.05, -2.43, 1.73, 0.12}; // Upstraight
static const double INIT_POS[] = {0.11, 0.11, -0.11, 0.11, 0.12}; // Home 
static const double Gripper_Init_Pos[] = {0.008, 0.008};

static const string JOINTNAME_PRE = "arm_joint_";
static const string GRIPPER_JOINTNAME_PRE = "gripper_finger_joint_";

static const uint NUM_ARM_JOINTS = 5;
static const uint NUM_GRIPPER_JOINTS = 2;

ros::Publisher armPositionsPublisher;
ros::Publisher gripperPositionPublisher;

vector<control_msgs::FollowJointTrajectoryActionGoal::ConstPtr> trajectories;


void trajectoryCallback(const control_msgs::FollowJointTrajectoryActionGoal::ConstPtr& msg)
{
  ROS_INFO("callback: Trajectory received");
  //cout << "Msg-Header" << endl << msg->header << endl;
  trajectories.push_back(msg);
}

void moveToInitPos(brics_actuator::JointPositions &command)
{
  std::stringstream jointName;
	
  for (int i = 0; i < NUM_ARM_JOINTS; ++i)
  {
    jointName.str("");
    jointName << JOINTNAME_PRE << (i + 1);

    command.positions[i].joint_uri = jointName.str();
    command.positions[i].value = INIT_POS[i];

    command.positions[i].unit = boost::units::to_string(boost::units::si::radians);
    cout << "Joint " << command.positions[i].joint_uri << " = " << command.positions[i].value << " " << command.positions[i].unit << endl;
  }
  
  cout << command << endl;
  armPositionsPublisher.publish(command); // trully send
  
  cout << "sending command for arm joint init position... and wait" << endl;

  ROS_INFO("End of arm_joint_init-pos");
  // ros::Duration(5).sleep();
}


void moveToGripperInitPos(brics_actuator::JointPositions &command)
{
  std::stringstream gripperJointName;
  
  for( int k = 0; k < NUM_GRIPPER_JOINTS; ++k)
  {
  	gripperJointName.str(""); 
  	(k == 0) ?	gripperJointName<< GRIPPER_JOINTNAME_PRE << "l": (gripperJointName<< GRIPPER_JOINTNAME_PRE << "r");
  		
    command.positions[k].joint_uri = gripperJointName.str();
    command.positions[k].value = Gripper_Init_Pos[k];
    command.positions[k].unit = boost::units::to_string(boost::units::si::meter);
    
    cout << "Joint " << command.positions[k].joint_uri << " = " << command.positions[k].value << " " << command.positions[k].unit << endl;
  }
  
  cout << command << endl;
  gripperPositionPublisher.publish(command); // trully send
  
  cout << "command for gripper joint init position... and wait" << endl;
  
  ROS_INFO("End of gripper_joint_init-pos");
}



int main(int argc, char **argv) {

  ros::init(argc, argv, "youbot_trajectory_controller");
	ros::NodeHandle n;
  uint loop_counter = 0;
  
  brics_actuator::JointPositions arm_command;
  brics_actuator::JointPositions gripper_command;
  
  vector <brics_actuator::JointValue> armJointPositions;
  vector <brics_actuator::JointValue> gripperJointPositions;
  
  armJointPositions.resize(NUM_ARM_JOINTS);
  gripperJointPositions.resize(NUM_GRIPPER_JOINTS);
  
  arm_command.positions = armJointPositions;
  gripper_command.positions = gripperJointPositions;
  
  armPositionsPublisher = n.advertise<brics_actuator::JointPositions > ("arm_1/arm_controller/position_command", 1);
  
  gripperPositionPublisher = n.advertise<brics_actuator::JointPositions> ("arm_1/gripper_controller/position_command", 1);
  ros::spinOnce();

  ros::Duration(1).sleep();
  ros::spinOnce();

  moveToInitPos(arm_command);
  moveToGripperInitPos(gripper_command);
  //ros::Duration(10).sleep();
  ROS_INFO("Init Pos should be reached");

  	return 0;
}

单独控制夹子

#include <ros/ros.h>

#include "brics_actuator/JointPositions.h"
#include "brics_actuator/JointValue.h"
#include "brics_actuator/Poison.h"

#include <boost/units/systems/si/length.hpp>
#include <boost/units/io.hpp>

#include <iostream>

using std::cout;
using std::endl;


int main(int argc, char** argv)
{
    ros::init(argc, argv, "gripper_move_test_node");

    ros::NodeHandle nh("~");
    ros::Publisher gPublisher = nh.advertise<brics_actuator::JointPositions>("/arm_1/gripper_controller/position_command", 1);

    double floatVal;
    if(argc !=2)
    {
        cout << "set to max open value: 0.011m." << endl;
         floatVal = 0.011;
    }
    else
    {
         floatVal = atof(argv[1]);
    }

    ros::Rate loop_rate(20);

    brics_actuator::JointPositions jp;
    brics_actuator::JointValue jvl;
    brics_actuator::JointValue jvr;
    brics_actuator::Poison myPoison;

/**************************
*
*topic： /arm_1/gripper_controller/position_command
*topic type: -----> brics_actuator/JointPositions

brics_actuator/Poison poisonStamp
  string originator
  string description
  float32 qos
brics_actuator/JointValue[] positions
  time timeStamp
  string joint_uri
  string unit
  float64 value
*
**************************/

    while(nh.ok())
    {
        myPoison.originator = "gripper_demo";
        myPoison.description = "demo";
		myPoison.qos = 0.0;
		jp.poisonStamp = myPoison;

        jvl.timeStamp = ros::Time::now();
		jvl.joint_uri = "gripper_finger_joint_l";
		jvl.unit = boost::units::to_string(boost::units::si::meter);
		jvl.value = floatVal;

//        jp.positions[0]=jvl;
        jp.positions.push_back (jvl);

        jvr.timeStamp = ros::Time::now();
		jvr.joint_uri = "gripper_finger_joint_r";
		jvr.unit = boost::units::to_string(boost::units::si::meter);
		jvr.value = floatVal;
//        jp.positions[1]=jvr;
        jp.positions.push_back (jvr);

		gPublisher.publish(jp);

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

监听Joint_states

#include <ros/ros.h>
#include <ros/console.h>
#include <fstream>
#include <ctime>
#include <boost/thread.hpp>
#include "sensor_msgs/JointState.h"
using namespace std;

/******
*topic： /joint_states 
*topic type：-----> sensor_msgs/JointState

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string[] name
float64[] position
float64[] velocity
float64[] effort

*******/
 char date[20];

void printCallback(const sensor_msgs::JointState::ConstPtr& msg)
{

    time_t t = msg->header.stamp.toSec();
    struct tm *tm = localtime(&t);

    strftime(date, sizeof(date), "%F %T", tm);
    cout<<date<<endl;

    for(int i=0; i < msg->name.size(); i++)
    {
        cout<<msg->name[i]<<"; ";
    }
    cout<<endl;

    for(int i=0; i < msg->position.size(); i++)
    {
        cout<<msg->position[i]<<"; ";
    }
    cout<<endl;

    for(int i=0; i < msg->velocity.size(); i++)
    {
        cout<<msg->velocity[i]<<"; ";
    }
    cout<<endl;

}

void save2fileCallback(const sensor_msgs::JointState::ConstPtr& msg)
{

        ofstream output_file ("～/catkin_ws/joint_states.txt", ios::out | ios::app | ios::ate);
        if (output_file.is_open())
        {
            cout << "Open out file successfully."<<endl;

            output_file << msg->header.stamp << endl;
            output_file << date << endl;

            for(int i=0; i < msg->name.size(); i++)
            {
                output_file<<msg->name[i]<<"; ";
            }
            output_file<<endl;


            for(int i=0; i < msg->position.size(); i++)
            {
                output_file<<msg->position[i]<<"; ";
            }
            output_file<<endl;

            for(int i=0; i < msg->velocity.size(); i++)
            {
                output_file<<msg->velocity[i]<<"; ";
            }
            output_file<<endl;
            output_file<<endl;

                  output_file.close();
            cout << "Writing down!" << endl;

        }else  cout << "Unable to open output file."<<endl;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "listen_joint_states_node");
  	ros::NodeHandle nh;

//     To erase old data.
    ofstream output_file0 ("～/catkin_ws/joint_states.txt",ios::out | ios::trunc);
    output_file0.close();

    ros::Subscriber sub = nh.subscribe("/joint_states", 10, printCallback);
    ros::Subscriber sub2 = nh.subscribe("/joint_states", 10, save2fileCallback);

    ros::AsyncSpinner s(2);
    s.start();

    ros::spin();

  	return 0;

}

输出文件的内容，注意这里的joint的名字不是按规律排出来的。千万注意。

3. 总结

3.1

这里我要说一下我在写这个cpp文件中遇到的一些问题，第一个遇到的就是gripper的名称问题，这里要注意的是在我们KUKA那个user Manual中40页那里，

我们的gipper必须是gripper_finger_joint_l或gripper_finger_joint_r哦，前缀那里不要瞎加

3.2

最开始我加入gripper的时候会发生core dump，因为以前学习GDB调试的时候遇到过这个，所以又花了些时间在GDB上。我们可以利用core-dump文件查看到出错的具体信息

当发生这个core dump的时候，内存访问错误，于是我开启GDB去调试一下，看看能否有具体原因：

gdb -c 后面的两个参数是core文件和可执行文件

仔细查看源文件，是访问了越界的数组而导致的core dump

修改完毕后执行rosrun youbot_ros_simple_trajectory youbot_ctl

输出结果

可以看到图中最后就是arm和gripper接收到的初始化信息，至此，我们和youbot_driver 的通信与控制就完成了。

参考资料：

• https://github.com/youbot/youbot-ros-pkg/blob/master/youbot_manipulation/simple_trajectory_controller/src/youbot_trajectory_controller.cpp