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

发布时间:2017年08月10日 17:42:28    浏览数:1014次    来自:信雪神话
​背景是京东2017 JRC X机器人挑战赛,比赛要求机器人系统从起始区出发,然后运行到货架位置,取出对应货架格子上的京东JOY,然后送往释放区域,释放完成后再返回起始区,再重新开始任务。

效果:

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

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

1.png

环境: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函数来进行文本内容的读取。

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

if streamObject_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节点进行抓取任务

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

2.png

3.png

下附完整代码

#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;  
//globals  
int 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=2  
bool nav_msg_rec_flag;     //data[0]=14  
bool nav_start_pos_flag;   //data[0]=0  
bool nav_column1_pos_flag; //data[0]=1  
bool nav_column2_pos_flag; //data[0]=2  
bool nav_column3_pos_flag; //data[0]=3  
bool nav_release_pos_flag; //data[0]=4  
bool nav_finished_flag;    //data[0]=15  
bool command_move_finished_flag=false;  
  
//2,kinect scan section,id=3  
bool kinect_scan_start_flag;       //data[0]=1  
bool kinect_scan_stop_flag;        //data[0]=0  
bool kinect_scan_fail_flag;        //data[0]=13  
bool kinect_msg_rec_flag;          //data[0]=14  
bool kinect_reset_flag=false;  
bool kinect_scan_finished_flag;    //data[0]=15  
  
//3,arm control section,id=4  
bool arm_start_fetch_flag;     //data[0]=1  
bool arm_stop_fetch_flag;      //data[0]=0  
bool arm_keep_fetch_flag;      //data[0]=2  
bool arm_release_obj_flag;     //data[0]=3  
bool arm_msg_rec_flag;         //data[0]=14  
bool 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 declaration  
  
int 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;  
    }  
  
}



标签: ROSClub机械臂

评论共0条评论

登录后再评论!

全部评论

目前没有评论