ROS下实现机器人序列任务的执行控制

效果：背景是京东2017 JRC X机器人挑战赛，比赛要求机器人系统从起始区出发，然后运行到货架位置，取出对应货架格子上的京东JOY，然后送往释放区域，释放完成后再返回起始区，再重新开始任务。

    我们最终完成的机器人系统如图所示。由于采样了传统的机械手机械臂+kinect+移动平台的方法，因此复杂度是所有队伍里最高的，也是最接近当下工业系统的解决方案，当然也是成本最高的。

环境：Ubuntu 14.04+ROS indigo+各组件配置环境。

[正文]

1 环境配置

    参考各个部件的环境配置，本节没有特殊需要的环境即需要EAI移动平台的环境配置，机械臂、机械手的环境配置，kinect V2的环境配置。

2 文件说明

该部分和机械手开发部分由于是一个人完成所以在同一个ros包内，文件结构相同。

beginner_tutorials下src中与比赛内容相关的如下：

launch/

  bhand_can_axis_control.launch带有6微力传感器配置的节点配置启动文件（最终使用）。

  bhand_force_control.launch使用基于bhand_controller控制方式的节点配置启动文件。

   src/

      bhand_axis_force_limit.cpp基于can总线操作的机械手控制（抓取，释放），6维力传感器驱动，为最终使用的节点文件。

      bhand_force_control.cpp基于bhand_controller的机械手控制。

     ge_test.cpp所有比赛任务流程控制，包括控制机器人移动到货架某个位置，控制kinect开始目标检测，机械臂开始目标规划与机械手抓取等，该节点应该是在确认其他节点工作正常后启动。

 

3 开发说明

3.1 获取目标行列位置

比赛开始时会给出需要拣选的目标joy的行列位置，我们通过txt文档来录入拣选信息。采样了sscanf函数来进行文本内容的读取。

先判断文件打开是否成功。

    ifstreamObject_position_file("/home/robot/test_row_column.txt");

   string temp;

   if(!Object_position_file.is_open())

    {

       ROS_ERROR("Failed to open JOY position file");

    }

   

    然后sscanf函数将数据读入数组，sscanf按格式读入，没有信息行要完全删除，不能仅仅删除数字（即留有空行），否则会出错。

   while(getline(Object_position_file,temp))

    {

       sscanf(temp.c_str(),"%d,%d",&row[obj_num],&column[obj_num]);

       obj_num++;

    }

3.2 发送部件启动指令

实际上主要执行两类内容，一个是给子节点线程发送启动相应功能指令，另一个是检测指令是否完成以及进行特殊状况处理，如给定的格子没有检测到目标物怎么处理。主要用到的函数如下所示，主要实现了函数内对于消息的回调和等待事件标志位。

其参数分别为需要发送的数据*notice_data指针，消息是否收到的标志指针*msg_rec_flag，当前动作是否完成的标志指针*finished_flag，以及话题发布与接受的类指针*notice_test。

intmain_MsgConform_ActFinishedWait(id_data_msgs::ID_Data *notice_data_test,bool*msg_rec_flag,bool *finished_flag,notice_pub_sub* notice_test)

{

    id_data_msgs::ID_Data notice_data;

    int loop_hz=10;

    ros::Rate loop_rate(loop_hz);

    //发送填充的数据

    notice_data_clear(&notice_data);

    notice_data.id=notice_data_test->id;

    for(int i=0;i<8;i++)notice_data.data[i]=notice_data_test->data[i];

    notice_test->notice_pub_sub_pulisher(notice_data);

    //data receive judge，数据接收判断

    int wait_count=0;

    while(ros::ok())

    {

 

        if(*msg_rec_flag==true)

        {

            *msg_rec_flag=false;

            break;

        }

        //如果没有收到子节点确认信息，则每10个运行周期发送一次，100个运行周期内仍然没有收到子线程的消息收到确认，则输出错误提示

        wait_count++;

        if(wait_count%10==0) //send msg againafter waiting 1s

        {

            switch (notice_data.id)

            {

                case 2:ROS_ERROR("Dashgodidn't receive msg,Retrying...");break;

                case 3:ROS_ERROR("Kinectdidn't receive msg,Retrying...");break;

                case 4:ROS_ERROR("Kinovaarm didn't receive msg,Retrying...");break;

                default:break;

            }

 

            notice_test->notice_pub_sub_pulisher(notice_data);

        }

        //100个运行周期为收到消息确认，输出错误提示

        if(wait_count>=100)

        {

            error_no=notice_data.id;

            wait_count=0;

            goto next;

        }

        notice_test->notice_sub_spinner(1);//notice话题数据接收的独立回调

        loop_rate.sleep();

    }

    //navigation action finish judge，如果收到信息接收确认，则等待子节点完成特定功能

    while(ros::ok())

    {

        if(*finished_flag==true)

        {

            *finished_flag=false;

            break;

        }

 

        notice_test->notice_sub_spinner(1); //notice话题数据接收的独立回调

        loop_rate.sleep();

    }

 

next:

    return error_no;

}

    上面代码段中subscriber的独立回调的技术在博文《基于ros---一个完整的实现topic 发布和监听的类和msg的简单使用（使用c++）》，效果是将类和subscriber的回调函数写在一个类里，并且使用独立的回调队列，而不会相互影响即多进程回调，好处是便于移植且不会影响其他函数回调，需要时单独指定回调就可以实现ros::spin()，并且在不需要时可以关闭某个函数的回调，只需要notice_test->notice_sub_spinner(0)参数为0即可。

3.3 未检测到目标处理

如果当前目标货架格kinect没有检测到目标，处理策略是将当前格存入待抓取目标序列数组的后一位，然后发送给移动底盘节点后退命令，然后进入下一个货架格的joy抓取处理。

 

ROS_INFO("2,informs kinect to scangrid shelves");

notice_data_clear(&notice_data);

notice_data.id=3;

notice_data.data[0]=1;

notice_data.data[1]=row[loop_sys_cnt];

notice_data.data[2]=column[loop_sys_cnt];

           error_no=main_MsgConform_ActFinishedWait(&notice_data,&kinect_msg_rec_flag,&kinect_scan_finished_flag,&notice_test);

error_deal(error_no);

if(kinect_reset_flag)//出现Kinect未检测到目标情况

{

    id_data_msgs::ID_Databack_move;

    notice_data_clear(&back_move);

    back_move.id=2;

    back_move.data[0]=6;

    back_move.data[1]=50;

    notice_test.notice_pub_sub_pulisher(back_move);//发送命令到移动底盘，回退50cm

    command_move_finished_flag=false;

    while(ros::ok())

    {

      notice_test.notice_sub_spinner(1);

       if(command_move_finished_flag)//等待移动底盘回退完成

       {

          command_move_finished_flag=false;

          ROS_INFO("Dashgo achieved move command!");

          break;

       }

    }

 

kinect_reset_flag=false;

continue;

}

4 操作说明

1）需要修改读取的文件的地址，填入需要抓取的目标行列。

2）确认其他节点工作正常。

3）启动ge_test节点进行抓取任务

给几张广为流传的机器人比赛中的图

下附完整代码

#include <iostream>#include <iomanip>#include <fstream>#include <ros/ros.h>#include "ros/callback_queue.h"#include <id_data_msgs/ID_Data.h>using namespace std;//globalsint error_no=0;int row[20]={0},column[20]={0};int obj_num=0;int loop_sys_cnt=0;int obj_cnt=0;//1,navigation section,id=2bool nav_msg_rec_flag;     //data[0]=14bool nav_start_pos_flag;   //data[0]=0bool nav_column1_pos_flag; //data[0]=1bool nav_column2_pos_flag; //data[0]=2bool nav_column3_pos_flag; //data[0]=3bool nav_release_pos_flag; //data[0]=4bool nav_finished_flag;    //data[0]=15bool command_move_finished_flag=false;//2,kinect scan section,id=3bool kinect_scan_start_flag;       //data[0]=1bool kinect_scan_stop_flag;        //data[0]=0bool kinect_scan_fail_flag;        //data[0]=13bool kinect_msg_rec_flag;          //data[0]=14bool kinect_reset_flag=false;bool kinect_scan_finished_flag;    //data[0]=15//3,arm control section,id=4bool arm_start_fetch_flag;     //data[0]=1bool arm_stop_fetch_flag;      //data[0]=0bool arm_keep_fetch_flag;      //data[0]=2bool arm_release_obj_flag;     //data[0]=3bool arm_msg_rec_flag;         //data[0]=14bool arm_act_finished_flag;    //data[0]=15//derfine int set_ontime=0;void notice_data_clear(id_data_msgs::ID_Data *test);class notice_pub_sub{public:    boost::function<void (const id_data_msgs::ID_Data::ConstPtr&)> notice_pub_sub_msgCallbackFun;    notice_pub_sub();    void notice_pub_sub_pulisher(id_data_msgs::ID_Data id_data);    void notice_display(id_data_msgs::ID_Data notice_msg,bool set);    void notice_sub_spinner(char set);private:    ros::NodeHandle notice_handle;    ros::Subscriber notice_subscriber;    ros::Publisher notice_publisher;    ros::SubscribeOptions notice_ops;    ros::AsyncSpinner *notice_spinner;    ros::CallbackQueue notice_callbackqueue;    void notice_msgCallback(const id_data_msgs::ID_Data::ConstPtr ¬ice_msg);};notice_pub_sub::notice_pub_sub(){    notice_pub_sub_msgCallbackFun=boost::bind(¬ice_pub_sub::notice_msgCallback,this,_1);    notice_ops=ros::SubscribeOptions::create<id_data_msgs::ID_Data>(                "/notice",                50,                notice_pub_sub_msgCallbackFun,                ros::VoidPtr(),                ¬ice_callbackqueue                );    notice_subscriber=notice_handle.subscribe(notice_ops);    notice_spinner=new ros::AsyncSpinner(1,¬ice_callbackqueue);    notice_publisher=notice_handle.advertise<id_data_msgs::ID_Data>("/notice",50);}void notice_pub_sub::notice_pub_sub_pulisher(id_data_msgs::ID_Data id_data){    notice_publisher.publish(id_data);}void notice_pub_sub::notice_display(id_data_msgs::ID_Data notice_msg,bool set){    if(set)    {        printf("REC Notice message,ID: %d,Data: ",notice_msg.id);        for(char i=0;i<8;i++)        {            printf("%d ",notice_msg.data[i]);            if(i==7) printf("\n");        }    }}void notice_pub_sub::notice_msgCallback(const id_data_msgs::ID_Data::ConstPtr ¬ice_msg){    id_data_msgs::ID_Data notice_message;    notice_message.id=0;    for(char i=0;i<8;i++)notice_message.data[i]=0;    notice_message.id=notice_msg->id;    for(char i=0;i<8;i++)notice_message.data[i]=notice_msg->data[i];    notice_pub_sub::notice_display(notice_message,true);    //1,navigation section    if(notice_message.id==2 && notice_message.data[0]==14)//msg received flag    {        nav_msg_rec_flag=true;    }    if(notice_message.id==2 && notice_message.data[0]==15)//nav finished flag    {        nav_finished_flag=true;    }    if(notice_message.id==2 && notice_message.data[0]==16)//nav finished flag    {        set_ontime++;        ROS_WARN("ON TIME No.%d",set_ontime);    }    if(notice_message.id==2 && notice_message.data[0]==8)    {        command_move_finished_flag=true;        ROS_INFO("Received dashgo command move finished flag");    }    //2,kinect scan section    if(notice_message.id==3 && notice_message.data[0]==14)//kinect msg received flag    {        kinect_msg_rec_flag=true;    }    if(notice_message.id==3 && notice_message.data[0]==15)//kinect scan finished flag    {        kinect_scan_finished_flag=true;    }    if(notice_message.id==3 && notice_message.data[0]==13)//kinect scan failed    {//        id_data_msgs::ID_Data id_data;//        id_data.id=3;//        for(int count=0;count<8;count++) id_data.data[count]=0;//        id_data.data[0]=1;//        notice_publisher.publish(id_data);        row[obj_num]=row[obj_cnt-1];        column[obj_num]=column[obj_cnt-1];        obj_num++;        kinect_reset_flag=true;        ROS_WARN("Received missing current column flag.Save data No.%d,Row:%d,Col%d",obj_cnt,row[obj_cnt-1],column[obj_cnt-1]);        kinect_scan_finished_flag=true;    }    //3,arm control section    if(notice_message.id==4 && notice_message.data[0]==14)//arm control msg received flag    {        arm_msg_rec_flag=true;    }    if(notice_message.id==4 && notice_message.data[0]==15)//arm fetch finished flag    {        arm_act_finished_flag=true;    }}void notice_pub_sub::notice_sub_spinner(char set){    if(set==1)        notice_spinner->start();    if(set==0)        notice_spinner->stop();}//function declarationint main_MsgConform_ActFinishedWait(id_data_msgs::ID_Data *notice_data_test,bool *msg_rec_flag,bool *finished_flag,notice_pub_sub* notice_test);void error_deal(int error_nu);int main(int argc,char **argv){    ros::init(argc,argv,"main_loop");    //read the position of JOY from the txt file    ifstream Object_position_file("/home/robot/test_row_column.txt");    string temp;    if(!Object_position_file.is_open())    {        ROS_ERROR("Failed to open JOY position file");    }    while(getline(Object_position_file,temp))    {        sscanf(temp.c_str(),"%d,%d",&row[obj_num],&column[obj_num]);        obj_num++;    }    cout<<"Object JOY number:"<<obj_num<<"\t"<<"JOY Object position:"<<endl;    for(int i=0;i<obj_num;i++)        cout<<"Column:"<<column[i]<<" "<<"Row:"<<row[i]<<endl;    Object_position_file.close();    //ROS Topic publish and event deal...    notice_pub_sub notice_test;    int loop_hz=10;    ros::Rate loop_rate(loop_hz);    id_data_msgs::ID_Data notice_data;    notice_data.id=0;    for(char i=0;i<8;i++) notice_data.data[i]=0;    int system_count=0;    while(ros::ok())    {        for(loop_sys_cnt=0;loop_sys_cnt<obj_num;loop_sys_cnt++)        {            printf("System run No.%d.\n",system_count++);            obj_cnt++;            //1,inform dashgo move to the column of object JOY position.            ROS_INFO("1,inform dashgo move to the column of object JOY position.");            notice_data_clear(¬ice_data);            notice_data.id=2;            notice_data.data[0]=column[loop_sys_cnt];            notice_data.data[1]=row[loop_sys_cnt];            error_no=main_MsgConform_ActFinishedWait(¬ice_data,&nav_msg_rec_flag,&nav_finished_flag,¬ice_test);            error_deal(error_no);            //2,informs kinect to scan grid shelves            //sleep(3);            ROS_INFO("2,informs kinect to scan grid shelves");            notice_data_clear(¬ice_data);            notice_data.id=3;            notice_data.data[0]=1;            notice_data.data[1]=row[loop_sys_cnt];            notice_data.data[2]=column[loop_sys_cnt];            error_no=main_MsgConform_ActFinishedWait(¬ice_data,&kinect_msg_rec_flag,&kinect_scan_finished_flag,¬ice_test);            error_deal(error_no);            if(kinect_reset_flag)            {                id_data_msgs::ID_Data back_move;                notice_data_clear(&back_move);                back_move.id=2;                back_move.data[0]=6;                back_move.data[1]=50;                notice_test.notice_pub_sub_pulisher(back_move);                command_move_finished_flag=false;                while(ros::ok())                {                    notice_test.notice_sub_spinner(1);                    if(command_move_finished_flag)                    {                        command_move_finished_flag=false;                        ROS_INFO("Dashgo achieved move command!");                        break;                    }                }                kinect_reset_flag=false;                continue;            }            //3,informs kinova arm to fetch object            ROS_INFO("3.1,informs kinova arm to fetch object");            notice_data_clear(¬ice_data);            notice_data.id=4;            notice_data.data[0]=1;            error_no=main_MsgConform_ActFinishedWait(¬ice_data,&arm_msg_rec_flag,&arm_act_finished_flag,¬ice_test);            error_deal(error_no);            //arm keep pose            ROS_INFO("3.2,Kinova Start being into Keep Pose ... ");            notice_data_clear(¬ice_data);            notice_data.id=4;            notice_data.data[0]=2;            error_no=main_MsgConform_ActFinishedWait(¬ice_data,&arm_msg_rec_flag,&arm_act_finished_flag,¬ice_test);            error_deal(error_no);            //4,inform dashgo move to the object release position.            ROS_INFO("4,inform dashgo move to the object release position.");            notice_data_clear(¬ice_data);            notice_data.id=2;            notice_data.data[0]=4;            error_no=main_MsgConform_ActFinishedWait(¬ice_data,&nav_msg_rec_flag,&nav_finished_flag,¬ice_test);            error_deal(error_no);            while(set_ontime<2){};set_ontime=0;            //5,inform kinova arm to release the object.            ROS_INFO("5,inform kinova arm to release the object.");            notice_data_clear(¬ice_data);            notice_data.id=4;            notice_data.data[0]=3;            error_no=main_MsgConform_ActFinishedWait(¬ice_data,&arm_msg_rec_flag,&arm_act_finished_flag,¬ice_test);            error_deal(error_no);            //6,inform dashgo move to the task start position.            ROS_INFO("6,inform dashgo move to the task start position.");            notice_data_clear(¬ice_data);            notice_data.id=2;            notice_data.data[0]=0;            error_no=main_MsgConform_ActFinishedWait(¬ice_data,&nav_msg_rec_flag,&nav_finished_flag,¬ice_test);            error_deal(error_no);            if(0==column[loop_sys_cnt] && 0==row[loop_sys_cnt])            {                ROS_INFO("Grasp tasks FINISHED!\n ");                break;            }        }        return 0;    }    return 0;}void notice_data_clear(id_data_msgs::ID_Data *test){    test->id=0;    for(int i=0;i<8;i++) test->data[i]=0;}int main_MsgConform_ActFinishedWait(id_data_msgs::ID_Data *notice_data_test,bool *msg_rec_flag,bool *finished_flag,notice_pub_sub* notice_test){    id_data_msgs::ID_Data notice_data;    int loop_hz=10;    ros::Rate loop_rate(loop_hz);    notice_data_clear(¬ice_data);    notice_data.id=notice_data_test->id;    for(int i=0;i<8;i++) notice_data.data[i]=notice_data_test->data[i];    notice_test->notice_pub_sub_pulisher(notice_data);    //data receive judge    int wait_count=0;    while(ros::ok())    {        if(*msg_rec_flag==true)        {            *msg_rec_flag=false;            break;        }        wait_count++;        if(wait_count%10==0) //send msg again after waiting 1s        {            switch (notice_data.id)            {                case 2:ROS_ERROR("Dashgo didn't receive msg,Retrying...");break;                case 3:ROS_ERROR("Kinect didn't receive msg,Retrying...");break;                case 4:ROS_ERROR("Kinova arm didn't receive msg,Retrying...");break;                default:break;            }            notice_test->notice_pub_sub_pulisher(notice_data);        }        if(wait_count>=100)        {            error_no=notice_data.id;            wait_count=0;            goto next;        }        notice_test->notice_sub_spinner(1);        loop_rate.sleep();    }    //navigation action finish judge    while(ros::ok())    {        if(*finished_flag==true)        {            *finished_flag=false;            break;        }        notice_test->notice_sub_spinner(1);        loop_rate.sleep();    }next:    return error_no;}void error_deal(int error_nu){    switch (error_nu)    {        case 2:        {            ROS_ERROR("Dashgo doesn't work normally!");            break;        }        case 3:        {            ROS_ERROR("Kinect doesn't work normally!");            break;        }        case 4:        {            ROS_ERROR("Kinova Arm doesn't work normally!");            break;        }        default:break;    }}