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

关注

原创最新推荐文章于 2026-08-11 14:06:22 发布 3.2k 阅读 AI 写同款· GEO 优化 ›

奥比中光深度相机（一）：环境配置深度相机可以获取RGB-D数据，不仅可以提取目标的可将光图像，同时还能获得目标距离相机的深度信息。深度相机在测距、三维重建、动物体尺体况分析等多个方面具有广泛的应用潜力。目前，市面上的深度相机包含多个品牌，包括Kinect系列、Intel RealSense系列以及奥比中光系列等，由于奥比中光相机开源了基于python的控制代码，大多深度学习的项目又都是基于python编写的，所以我们选择了奥比中光深度相机Gemini 2L型号。阅读详情

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后编译例子步骤：

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

cd SDK路径/samples/

mkdir build

cd build

用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_ = BufferPtr(new uint8_t[byteLength]);

std::memset(displayBuffer_.get(), 0, byteLength);

texture_.create(depthWidth_, depthHeight_);

sprite_.setTexture(texture_, true);

sprite_.setPosition(0, 0);

}

void init_overlay_texture(int width, int height)

{

if (overlayBuffer_ == nullptr || width != overlayWidth_ || height != overlayHeight_)

{

overlayWidth_ = width;

overlayHeight_ = height;

int byteLength = overlayWidth_ * overlayHeight_ * 4;

overlayBuffer_ = BufferPtr(new uint8_t[byteLength]);

std::fill(&overlayBuffer_[0], &overlayBuffer_[0] + byteLength, 0);

overlayTexture_.create(overlayWidth_, overlayHeight_);

overlaySprite_.setTexture(overlayTexture_, true);

overlaySprite_.setPosition(0, 0);

}

void check_fps()

{

double fpsFactor = 0.02;

std::clock_t newTimepoint= std::clock();

long double frameDuration = (newTimepoint - lastTimepoint_) / static_cast<long double>(CLOCKS_PER_SEC);

frameDuration_ = frameDuration * fpsFactor + frameDuration_ * (1 - fpsFactor);

lastTimepoint_ = newTimepoint;

double fps = 1.0 / frameDuration_;

// printf("FPS: %3.1f (%3.4Lf ms)\n", fps, frameDuration_ * 1000);

}

void processDepth(astra::Frame& frame)

{

const astra::DepthFrame depthFrame = frame.get<astra::DepthFrame>();

if (!depthFrame.is_valid()) { return; }

int width = depthFrame.width();

int height = depthFrame.height();

init_depth_texture(width, height);

const int16_t* depthPtr = depthFrame.data();

for(int y = 0; y < height; y++)

{

for(int x = 0; x < width; x++)

{

int index = (x + y * width);

int index4 = index * 4;

int16_t depth = depthPtr[index];

uint8_t value = depth % 255;

displayBuffer_[index4] = value;

displayBuffer_[index4 + 1] = value;

displayBuffer_[index4 + 2] = value;

displayBuffer_[index4 + 3] = 255;

}

texture_.update(displayBuffer_.get());

}

void processBodies(astra::Frame& frame)

{

astra::BodyFrame bodyFrame = frame.get<astra::BodyFrame>();

jointPositions_.clear();

circles_.clear();

circleShadows_.clear();

boneLines_.clear();

boneShadows_.clear();

if (!bodyFrame.is_valid() || bodyFrame.info().width() == 0 || bodyFrame.info().height() == 0)

{

clear_overlay();

return;

}

const float jointScale = bodyFrame.info().width() / 120.f;

const auto& bodies = bodyFrame.bodies();

for (auto& body : bodies)

{

// bodyFrame.frame_index(), body.id(), unsigned(body.hand_poses().left_hand()));

for(auto& joint : body.joints())

{

onInit();

astra::JointType jointType = joint.type();

if(jointType == astra::JointType::ShoulderSpine)

{

shoulderSpinePosition = joint.world_position();

}

if(jointType == astra::JointType::LeftHand)

{

leftHandPosition = joint.world_position();

}

if(jointType == astra::JointType::RightHand)

{

rightHandPosition = joint.world_position();

}

//set state

if(leftHandPosition.y - shoulderSpinePosition.y > 300)

{

timeState++;

std_msgs::String msg;

std::string ss;

ss = "状态变为一,开始跟随" ;

msg.data = ss;

pubVoice.publish(msg);

state = 1 ;

}

if(rightHandPosition.y - shoulderSpinePosition.y > 300)

{

timeState++ ;

std_msgs::String msg;

std::string ss;

ss = "状态变为二,等待十五秒" ;

msg.data = ss;

pubVoice.publish(msg);

state = 2 ;

}

if(leftHandPosition.y>-100 && leftHandPosition.y<100 && rightHandPosition.y<100 && rightHandPosition.y>-100)

{

timeState++ ;

std_msgs::String msg;

std::string ss;

ss = "状态变为零,停止跟随" ;

msg.data = ss;

if(timeState % 100 == 0)

{

pubVoice.publish(msg);

state = 0 ;

}

if(jointType == astra::JointType::MidSpine)

{

auto worldPosition = joint.world_position();

if(worldPosition.x / 500.f>0.2 || worldPosition.x / 500.f<-0.2)

{

if(worldPosition.x / 500.f>0.3)

{

angle = 0.3;

}else if(worldPosition.x / 500.f<-0.3)

{

angle = -0.3;

}else

{

angle = worldPosition.x / 500.f ;

}

} else

{

if(worldPosition.x / 1000.f>0.3)

{

angle = 0.3;

}else if(worldPosition.x / 1000.f<-0.3)

{

angle = -0.3;

}else

{

angle = worldPosition.x / 1000.f ;

}

if((worldPosition.z - 1500.f) / 500.f > 0.2 || (worldPosition.z - 1500.f) / 500.f<-0.2)

{

if((worldPosition.z -1500.f) / 500.f>0.3)

{

line = 0.3;

}else if((worldPosition.z -1500.f) / 500.f<-0.3)

{

line = -0.3;

}else

{

line = (worldPosition.z -1500.f) / 500.f ;

}

}else

{

if((worldPosition.z -1500.f) / 1000.f>0.3)

{

line = 0.3;

}else if((worldPosition.z -1500.f) / 1000.f<-0.3)

{

line = -0.3;

}else

{

line = (worldPosition.z -1500.f) / 1000.f ;

}

publishCmdVel();

printf("z:%d\n",state);

}

jointPositions_.push_back(joint.depth_position());

}

update_body(body, jointScale);

}

const auto& floor = bodyFrame.floor_info(); //floor

if (floor.floor_detected())

{

const auto& p = floor.floor_plane();

std::cout << "Floor plane: ["

<< p.a() << ", " << p.b() << ", " << p.c() << ", " << p.d()

<< "]" << std::endl;

}

const auto& bodyMask = bodyFrame.body_mask();

const auto& floorMask = floor.floor_mask();

update_overlay(bodyMask, floorMask);

}

void update_body(astra::Body body,

const float jointScale)

{

const auto& joints = body.joints();

if (joints.empty())

{

return;

}

for (const auto& joint : joints)

{

astra::JointType type = joint.type();

const auto& pos = joint.depth_position();

if (joint.status() == astra::JointStatus::NotTracked)

{

continue;

}

auto radius = jointRadius_ * jointScale; // pixels

sf::Color circleShadowColor(0, 0, 0, 255);

auto color = sf::Color(0x00, 0xFF, 0x00, 0xFF);

if ((type == astra::JointType::LeftHand && astra::HandPose::Grip==body.hand_poses().left_hand()) ||

(type == astra::JointType::RightHand && astra::HandPose::Grip==body.hand_poses().right_hand()))

{

radius *= 1.5f;

circleShadowColor = sf::Color(255, 255, 255, 255);

color = sf::Color(0x00, 0xAA, 0xFF, 0xFF);

}

const auto shadowRadius = radius + shadowRadius_ * jointScale;

const auto radiusDelta = shadowRadius - radius;

sf::CircleShape circle(radius);

circle.setFillColor(sf::Color(color.r, color.g, color.b, 255));

circle.setPosition(pos.x - radius, pos.y - radius);

circles_.push_back(circle);

sf::CircleShape shadow(shadowRadius);

shadow.setFillColor(circleShadowColor);

shadow.setPosition(circle.getPosition() - sf::Vector2f(radiusDelta, radiusDelta));

circleShadows_.push_back(shadow);

}

update_bone(joints, jointScale, astra::JointType::Head, astra::JointType::ShoulderSpine);

update_bone(joints, jointScale, astra::JointType::ShoulderSpine, astra::JointType::LeftShoulder);

update_bone(joints, jointScale, astra::JointType::LeftShoulder, astra::JointType::LeftElbow);

update_bone(joints, jointScale, astra::JointType::LeftElbow, astra::JointType::LeftHand);

update_bone(joints, jointScale, astra::JointType::ShoulderSpine, astra::JointType::RightShoulder);

update_bone(joints, jointScale, astra::JointType::RightShoulder, astra::JointType::RightElbow);

update_bone(joints, jointScale, astra::JointType::RightElbow, astra::JointType::RightHand);

update_bone(joints, jointScale, astra::JointType::ShoulderSpine, astra::JointType::MidSpine);

update_bone(joints, jointScale, astra::JointType::MidSpine, astra::JointType::BaseSpine);

update_bone(joints, jointScale, astra::JointType::BaseSpine, astra::JointType::LeftHip);

update_bone(joints, jointScale, astra::JointType::LeftHip, astra::JointType::LeftKnee);

update_bone(joints, jointScale, astra::JointType::LeftKnee, astra::JointType::LeftFoot);

update_bone(joints, jointScale, astra::JointType::BaseSpine, astra::JointType::RightHip);

update_bone(joints, jointScale, astra::JointType::RightHip, astra::JointType::RightKnee);

update_bone(joints, jointScale, astra::JointType::RightKnee, astra::JointType::RightFoot);

}

void update_bone(const astra::JointList& joints,

const float jointScale,astra::JointType j1,

astra::JointType j2)

{

const auto& joint1 = joints[int(j1)];

const auto& joint2 = joints[int(j2)];

if (joint1.status() == astra::JointStatus::NotTracked ||

joint2.status() == astra::JointStatus::NotTracked)

{

return;

}

const auto& jp1 = joint1.depth_position();

const auto& jp2 = joint2.depth_position();

auto p1 = sf::Vector2f(jp1.x, jp1.y);

auto p2 = sf::Vector2f(jp2.x, jp2.y);

sf::Color color(255, 255, 255, 255);

float thickness = lineThickness_ * jointScale;

if (joint1.status() == astra::JointStatus::LowConfidence ||

joint2.status() == astra::JointStatus::LowConfidence)

{

color = sf::Color(128, 128, 128, 255);

thickness *= 0.5f;

}

boneLines_.push_back(sfLine(p1,

p2,

color,

thickness));

const float shadowLineThickness = thickness + shadowRadius_ * jointScale * 2.f;

boneShadows_.push_back(sfLine(p1,

p2,

sf::Color(0, 0, 0, 255),

shadowLineThickness));

}

void update_overlay(const astra::BodyMask& bodyMask,

const astra::FloorMask& floorMask)

{

const auto* bodyData = bodyMask.data();

const auto* floorData = floorMask.data();

const int width = bodyMask.width();

const int height = bodyMask.height();

init_overlay_texture(width, height);

const int length = width * height;

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

{

const auto bodyId = bodyData[i];

const auto isFloor = floorData[i];

sf::Color color(0x0, 0x0, 0x0, 0x0);

if (bodyId != 0)

{

color = get_body_color(bodyId);

}

else if (isFloor != 0)

{

color = sf::Color(0x0, 0x0, 0xFF, 0x88);

}

const int rgbaOffset = i * 4;

overlayBuffer_[rgbaOffset] = color.r;

overlayBuffer_[rgbaOffset + 1] = color.g;

overlayBuffer_[rgbaOffset + 2] = color.b;

overlayBuffer_[rgbaOffset + 3] = color.a;

}

overlayTexture_.update(overlayBuffer_.get());

}

void clear_overlay()

{

int byteLength = overlayWidth_ * overlayHeight_ * 4;

std::fill(&overlayBuffer_[0], &overlayBuffer_[0] + byteLength, 0);

overlayTexture_.update(overlayBuffer_.get());

}

virtual void on_frame_ready(astra::StreamReader& reader,

astra::Frame& frame) override

{

processDepth(frame);

processBodies(frame);

check_fps();

}

void draw_bodies(sf::RenderWindow& window)

{

const float scaleX = window.getView().getSize().x / overlayWidth_;

const float scaleY = window.getView().getSize().y / overlayHeight_;

sf::RenderStates states;

sf::Transform transform;

transform.scale(scaleX, scaleY);

states.transform *= transform;

for (const auto& bone : boneShadows_)

window.draw(bone, states);

for (const auto& c : circleShadows_)

window.draw(c, states);

for (const auto& bone : boneLines_)

window.draw(bone, states);

for (auto& c : circles_)

window.draw(c, states);

}

void draw_to(sf::RenderWindow& window)

{

if (displayBuffer_ != nullptr)

{

const float scaleX = window.getView().getSize().x / depthWidth_;

const float scaleY = window.getView().getSize().y / depthHeight_;

sprite_.setScale(scaleX, scaleY);

window.draw(sprite_); // depth

}

if (overlayBuffer_ != nullptr)

{

const float scaleX = window.getView().getSize().x / overlayWidth_;

const float scaleY = window.getView().getSize().y / overlayHeight_;

overlaySprite_.setScale(scaleX, scaleY);

window.draw(overlaySprite_); //bodymask and floormask

}

draw_bodies(window);

}

void publishCmdVel()

{

geometry_msgs::Twist twist;

if(state == 1)

{

twist.linear.x = line;

twist.angular.z = angle;

pub.publish(twist);

} else if(state == 2)

{

twist.linear.x = 0;

twist.angular.z = 0;

pub.publish(twist);

clock_t delay;

delay = 90 * 100000 ;

clock_t start = clock();

while(clock()-start < delay){

state = 1;

}

std_msgs::String msg;

std::string ss;

ss = "状态变为一,开始跟随" ;

msg.data = ss;

pubVoice.publish(msg);

}else

{

twist.linear.x = 0;

twist.angular.z = 0;

pub.publish(twist);

}

private:

long double frameDuration_{ 0 };

std::clock_t lastTimepoint_ { 0 };

sf::Texture texture_;

sf::Sprite sprite_;

using BufferPtr = std::unique_ptr < uint8_t[] >;

BufferPtr displayBuffer_{ nullptr };

std::vector<astra::Vector2f> jointPositions_;

int depthWidth_{0};

int depthHeight_{0};

int overlayWidth_{0};

int overlayHeight_{0};

std::vector<sfLine> boneLines_;

std::vector<sfLine> boneShadows_;

std::vector<sf::CircleShape> circles_;

std::vector<sf::CircleShape> circleShadows_;

float lineThickness_{ 0.5f }; // pixels

float jointRadius_{ 1.0f }; // pixels

float shadowRadius_{ 0.5f }; // pixels

BufferPtr overlayBuffer_{ nullptr };

sf::Texture overlayTexture_;

sf::Sprite overlaySprite_;

};

astra::DepthStream configure_depth(astra::StreamReader& reader)

{

auto depthStream = reader.stream<astra::DepthStream>();

astra::ImageStreamMode depthMode;

depthMode.set_width(640);

depthMode.set_height(480);

depthMode.set_pixel_format(astra_pixel_formats::ASTRA_PIXEL_FORMAT_DEPTH_MM);

depthMode.set_fps(30);

depthStream.set_mode(depthMode);

return depthStream;

}

int main(int argc, char** argv)

{

ros::init(argc , argv ,"skeleton_follow");

astra::initialize();

const char* licenseString = "<INSERT LICENSE KEY HERE>";

orbbec_body_tracking_set_license(licenseString);

sf::RenderWindow window(sf::VideoMode(1280, 960), "Simple Body Viewer");

#ifdef _WIN32

auto fullscreenStyle = sf::Style::None;

#else

auto fullscreenStyle = sf::Style::Fullscreen;

#endif

const sf::VideoMode fullScreenMode = sf::VideoMode::getFullscreenModes()[0];

const sf::VideoMode windowedMode(1280, 960);

bool isFullScreen = false;

astra::StreamSet sensor;

astra::StreamReader reader = sensor.create_reader();

BodyVisualizer listener;

auto depthStream = configure_depth(reader);

depthStream.start();

reader.stream<astra::BodyStream>().start();

reader.add_listener(listener);

BodyVisualizer bv ;

while (window.isOpen())

{

astra_update();

sf::Event event;

while (window.pollEvent(event))

{

switch (event.type)

{

case sf::Event::Closed:

window.close();

break;

case sf::Event::KeyPressed:

{

if (event.key.code == sf::Keyboard::C && event.key.control)

{

window.close();

}

switch (event.key.code)

{

case sf::Keyboard::Escape:

window.close();

break;

case sf::Keyboard::F:

if (isFullScreen)

{

window.create(windowedMode, "Simple Body Viewer", sf::Style::Default);

}

else

{

window.create(fullScreenMode, "Simple Body Viewer", fullscreenStyle);

}

isFullScreen = !isFullScreen;

break;

case sf::Keyboard::R:

depthStream.enable_registration(!depthStream.registration_enabled());

break;

case sf::Keyboard::M:

depthStream.enable_mirroring(!depthStream.mirroring_enabled());

break;

default:

break;

}

break;

}

default:

break;

}

// clear the window with black color

window.clear(sf::Color::Black);

listener.draw_to(window);

window.display();

}

astra::terminate();

return 0;

}

收录于

机器人

ros

标签

#follow

OpenNI2 SDK for ROS2奥比中光深度摄像头驱动安装配置进入到下载好的SDK for ROS2 SDK目录中（需要根据自己情况修改，我的路径是：~/Downloads/OpenNI_SDK_ROS2_v1.0.2_linux）。以下主要针对Astra S系列的驱动安装，用于使用ROS2的开发，供大家参考，提供了驱动的下载地址。sudo ldconfig # 刷新链接库。sudo ldconfig # 刷新链接库。sudo ldconfig # 刷新链接库。之后输入以下指令即可测试摄像头了。安装libusb rules。安装magic_enum。阅读详情

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

相关推荐