ROS多用户并发demo

前言

嗨，各位小伙子，这节我们来写一个简单的多用户demo，并且每个用户都可以使用ROS库。

你可以把SLAM程序想成一台车，每个用户都有一把钥匙，他可以选择开或者不开。

构想的框图如下：

那么问题来了，本地只有一套SLAM代码，SLAM代码又由很多ROS node组成，咋办？要知道ros node可是要用命令行打开的。说实话，对ROS不熟悉的我，也很绝望，我刚开始想的就是把SLAM代码封装成一个函数，定义在用户类里，然后实例化用户，直接调用就好了，可是我发现这个node很烦，我没法在一个node里创建另一个node（但是节点是可以的），所以我就转换了一种方式，不要node了，其实我觉得用不用node也没差。

原本的想法：
把含有多个ros node的SLAM系统封装成一个函数；
现在的尝试：
放弃在一个node里创建另一个node的想法，每个用户直接跑独立的线程。就只有主节点一个node，SLAM的系统去掉node，封装成函数，但是ros相关函数还是能用的，node和节点不冲突。

于是我就先写了一个简单的demo，ffmpeg拉流部分打算下一节再写。

程序

mian_node.cpp

#include <iostream>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "User.h"
vector<NodeInNode *> user;

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

    ros::NodeHandle n;

    ros::Rate loop_rate(1);
    int User_num = 0;

    for(int i = 0; i < 5 ; i++)
    {
        NodeInNode *k = new NodeInNode(&i);
        user.push_back(k);
        user[i]->UserID = i;
        loop_rate.sleep();
    }
    int current_id ;
    int flag = 1;
    int in_id;
    while(ros::ok())
    {
        cout << "请输入用户ID :" ;
        cin >> in_id;
        loop_rate.sleep();
        if(flag==1)
        {
            //在这里循环检测是否有用户ID
            ROS_INFO("input ID is : %d /n",in_id);

            for(int i = 0; i < user.size(); i ++)
            {
                if(user[i]->UserID == in_id)
                {
                    current_id = i;
                    break;
                }
                //到末尾了，还没有找到
                if(i == user.size()-1)
                {
                    current_id = user.size();
                }
            }
            //没有就新建一个
            if(current_id == user.size())
            {
                //current_id = user.size();
                //其实新节点先不要开启线程会比较好
                NodeInNode *k = new NodeInNode(&current_id); //新内存
                user.push_back(k);
                user[current_id]->UserID = in_id;
                ROS_INFO("register user success!, this User`s ID is : %d /n",user[current_id]->UserID);
                //做相关操作，比如说创建用户文件夹等，在这个demo里是打开线程,打印一下
                user[current_id]->startpthread(&current_id);
            }
            //已经有这个用户
            else
            {
               //做相关操作 在这个demo里是打开线程，然后打印一下
               user[current_id]->startpthread(&current_id);
            }
        }
        loop_rate.sleep();
    }
    return 0;
}

user.h

#ifndef CLASS_ROS_NODEINNODE_H
#define CLASS_ROS_NODEINNODE_H
#include <iostream>
#include "ros/ros.h"
#include "std_msgs/String.h"
using namespace std;
class NodeInNode {
public:
    NodeInNode(int * i);
    ~NodeInNode();
    int UserID;
    int startpthread(int *index);
};
extern vector<NodeInNode *> user;


#endif //CLASS_ROS_NODEINNODE_H

user.cpp

#include "User.h"


NodeInNode::NodeInNode(int *i) {
    ROS_INFO("pthread init init init init %d /n",*i);
//    pthread_t test;
//    int ret = pthread_create(&test,NULL,thread,(void *)i);   //开启一个线程
}


NodeInNode::~NodeInNode() {}

void *thread(void *ptr)
{
    int count = 0;
    ros::Rate loop_rate(1);
    int num = *(int *)ptr;
    ROS_INFO("My UserId is : %d \n",user[num]->UserID);
    //do something you want
    while(1)
    {
        loop_rate.sleep();
        //ROS_INFO("My UserId is : %d , the number is %d",user[num]->UserID,count);
        count ++;
    }
    return 0;
}
int NodeInNode::startpthread(int *num)
{
    pthread_t test;
    int ret = pthread_create(&test,NULL,thread,(void *)num);   //开启一个线程
    if(ret < 0)
    {
        return -1;
    }
    return 0;
}

测试结果

其中，线程使用到了linux独有的pthread，当然，你用std::thread去创建线程也是可以的。

结论

恐怕还是有点难上述所示框图还有点难，慢慢来吧。

如果我的文章对你有帮助，欢迎关注，点赞，评论。