奥比中光深度摄像头实现follow

Github地址：https://github.com/robotBing/skeleton_follow

演示视频地址：http://blog.jiutucao.com:9000/upload/2018/07/skeleton_follow.mp4

  奥比中光摄像头是有ros驱动的，最开始的点云颜色跟随就是直接调用奥比提供的astra_launch包，但是骨骼跟随自己需要做的工作就多得多了。不得不说过程很艰难，使用一门不是最擅长的语言，没有ide，只能用记事本进行代码编写，调试全靠打印信息。时间又紧，只能做出来一个半成品。

  下面说一下开发时的心路历程，先到astra的官网上下载奥比中光的sdk，这个sdk是可以进行骨骼识别的，但是和ros无关，我要做的就是根据提供demo代码，筛选出我需要的数据，对数据进行处理，映射成为ros需要的速度数据，最后将速度数据发布到cmd_vel话题上。最烦的就是没有ide，调用哪个方法要一点一点的试，全靠手打。

  Sdk提供的骨骼识别有19个关节信息，核心程序的思路就是提取身体最中心的关节坐标，映射为速度信息，提取手臂各个关节的坐标，对人的动作进行判断，让机器人在等待15s，开始跟随，停止跟随几个状态转换。程序有两个可以完善的地方1.程序没有使用多线程，等待15s过程中，整个线程停止，导致整个程序停止.2.动作的判断不够全面，受环境干扰太大。两个问题都是致命的，但是修改起来很简单，我就不做改进了，希望学弟们能把程序完善。

sdk下载地址：https://orbbec3d.com/develop

sdk中的demo编译需要sfml支持，下载sfml：

sudo apt-get install libsfml-dev

 

sfml官网教程地址：https://www.sfml-dev.org/tutorials/2.0/start-linux.php

下载sdk后编译例子步骤：

1. 进入SDK下的samples目录下，新建一个build目录，在终端中进入到build目录下。

cd SDK路径/samples/

mkdir build

cd build

 

1. 用cmake编译并转到bin目录下，运行测试程序。

cmake ..

make

编译后的可执行文件在build目录下的bin目录里

  下面是核心代码：

#include <SFML/Graphics.hpp>

#include <astra/astra.hpp>

#include <iostream>

#include <cstring>

#include "ros/ros.h"

#include "std_msgs/String.h"

#include <geometry_msgs/Twist.h>

#include <string>

#include <ctime>

#include <sstream>

class sfLine : public sf::Drawable

{

public:

    sfLine(const sf::Vector2f& point1, const sf::Vector2f& point2, sf::Color color, float thickness)

        : color_(color)

    {

        const sf::Vector2f direction = point2 - point1;

        const sf::Vector2f unitDirection = direction / std::sqrt(direction.x*direction.x + direction.y*direction.y);

        const sf::Vector2f normal(-unitDirection.y, unitDirection.x);

        const sf::Vector2f offset = (thickness / 2.f) * normal;

        vertices_[0].position = point1 + offset;

        vertices_[1].position = point2 + offset;

        vertices_[2].position = point2 - offset;

        vertices_[3].position = point1 - offset;

 

        for (int i = 0; i<4; ++i)

            vertices_[i].color = color;

    }

 

    void draw(sf::RenderTarget &target, sf::RenderStates states) const

    {

        target.draw(vertices_, 4, sf::Quads, states);

    }

private:

    sf::Vertex vertices_[4];

    sf::Color color_;

};

class BodyVisualizer : public astra::FrameListener

{

public:

      float line = 0;

      float angle = 0;

      ros::Publisher pub;

      ros::Publisher pubVoice;

      astra::Vector3f shoulderSpinePosition;

      astra::Vector3f leftHandPosition;

      astra::Vector3f rightHandPosition;

      int state = 0 ;

      int timeState = 1;

      virtual void onInit()

      {

      ros::NodeHandle n ;

      pub = n.advertise<geometry_msgs::Twist>("cmd_vel",1);

      pubVoice = n.advertise<std_msgs::String>("speak_string",1);

      }

    static sf::Color get_body_color(std::uint8_t bodyId)

    {

        if (bodyId == 0)

        {

            return sf::Color(0x00, 0x00, 0x00, 0x00);

        }

        // Case 0 below could mean bodyId == 25 or

        // above due to the "% 24".

        switch (bodyId % 24) {

        case 0:

            return sf::Color(0x00, 0x88, 0x00, 0xFF);

        case 1:

            return sf::Color(0x00, 0x00, 0xFF, 0xFF);

        case 2:

            return sf::Color(0x88, 0x00, 0x00, 0xFF);

        case 3:

            return sf::Color(0x00, 0xFF, 0x00, 0xFF);

        case 4:

            return sf::Color(0x00, 0x00, 0x88, 0xFF);

        case 5:

            return sf::Color(0xFF, 0x00, 0x00, 0xFF);

 

        case 6:

            return sf::Color(0xFF, 0x88, 0x00, 0xFF);

        case 7:

            return sf::Color(0xFF, 0x00, 0xFF, 0xFF);

        case 8:

            return sf::Color(0x88, 0x00, 0xFF, 0xFF);

        case 9:

            return sf::Color(0x00, 0xFF, 0xFF, 0xFF);

        case 10:

            return sf::Color(0x00, 0xFF, 0x88, 0xFF);

        case 11:

            return sf::Color(0xFF, 0xFF, 0x00, 0xFF);

 

        case 12:

            return sf::Color(0x00, 0x88, 0x88, 0xFF);

        case 13:

            return sf::Color(0x00, 0x88, 0xFF, 0xFF);

        case 14:

            return sf::Color(0x88, 0x88, 0x00, 0xFF);

        case 15:

            return sf::Color(0x88, 0xFF, 0x00, 0xFF);

        case 16:

            return sf::Color(0x88, 0x00, 0x88, 0xFF);

        case 17:

            return sf::Color(0xFF, 0x00, 0x88, 0xFF);

 

        case 18:

            return sf::Color(0xFF, 0x88, 0x88, 0xFF);

        case 19:

            return sf::Color(0xFF, 0x88, 0xFF, 0xFF);

        case 20:

            return sf::Color(0x88, 0x88, 0xFF, 0xFF);

        case 21:

            return sf::Color(0x88, 0xFF, 0xFF, 0xFF);

        case 22:

            return sf::Color(0x88, 0xFF, 0x88, 0xFF);

        case 23:

            return sf::Color(0xFF, 0xFF, 0x88, 0xFF);

        default:

            return sf::Color(0xAA, 0xAA, 0xAA, 0xFF);

        }

    }

    void init_depth_texture(int width, int height)

    {

        if (displayBuffer_ == nullptr || width != depthWidth_ || height != depthHeight_)

        {

            depthWidth_ = width;

            depthHeight_ = height;

            int byteLength = depthWidth_ * depthHeight_ * 4;

            displayBuffer