大疆onboard-SDK-ROS的安装编译以及rosbag record 视频
运行ROS 示例代码：
一、编译示例代码
1.创建ROS 工作空间
mkdir ws_djiCamera
cd ws_djiCamera
mkdir src
cd src
catkin_init_workspace
使用如上命令创建工作空间后，请将OSDK 示例代码复制在src目录下。
2.编译示例代码
可参考：配置开发环境
在ws_djiCamera 目录下，使用catkin_make 命令，编译OSDK 示例代码。
配置当前工作空间source devel/setup.bash
二、执行示例程序
1.补充应用信息
编辑配置文件XXXX.launch，替换应用的ID、Key和波特率等信息。
OSDK ROS 4.0 节点配置文件的路径：
/path/to/catkin_ws/src/Onboard-SDK-ROS/launch/dji_vehicle_node.launch
2.运行ROS 的主节点：
OSDK ROS 4.0：roslaunch dji_osdk_ros dji_vehicle_node.launch
3.运行示例节点
OSDK ROS 4.0 打开新的终端窗口，进入catkin_ws目录下，使用如下命令配置工作空间(以运行“无人机飞行控制”程序为例)：
$source devel/setup.bash
$rosrun dji_osdk_ros flight_control_node
但是此次主要是为了使用视频输出功能：rosrun dji_osdk_ros camera_stream_node
1、需要修改路径：/home/foia/ws_djiCamera/src/Onboard-SDK-ROS下
的CMakeLists.txt文件和package.xml文件，如下：
①.CMakeLists.txt文件中需要添加：
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
message_generation
nav_msgs
nmea_msgs
roscpp
rospy
std_msgs
sensor_msgs
message_filters # for message_filter
cv_bridge //****需要添加****
image_transport //****需要添加****
)
②.package.xml文件中需要添加：
<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>image_transport</exec_depend>
【注释】
ros中图像格式与opencv中图像格式之间的转换使用cv_bridge
图片消息的发布使用image_transport
2、需要修改源程序camera_stream_node.cpp文件，如下：
/** @file camera_stream_node.cpp
* @version 4.0
* @date June 2020
*
* @brief sample node of camera stream.
*
* @Copyright (c) 2020 DJI
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*
*/
//INCLUDE
#include <ros/ros.h>
#include <iostream>
#include <dji_camera_image.hpp>
#include <dji_osdk_ros/SetupCameraStream.h>
#include <dji_osdk_ros/SetupCameraH264.h>
#include <sensor_msgs/Image.h>
//****为了使用cv_bridge与image_transport****
//****cv_bridge用于opencv的图片格式与ros下的图片格式进行转换****
//****image_transport用于发布图片格式的消息****
#include <sensor_msgs/CompressedImage.h>
//#include <sensor_msgs/image_encodeings.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
extern "C"{
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libswscale/swscale.h>
}
#ifdef OPEN_CV_INSTALLED
#include "opencv2/opencv.hpp"
#include "opencv2/highgui/highgui.hpp"
#endif
//CODE
using namespace dji_osdk_ros;
AVCodecContext* pCodecCtx;
AVCodec* pCodec;
AVCodecParserContext* pCodecParserCtx;
SwsContext* pSwsCtx;
AVFrame* pFrameYUV;
AVFrame* pFrameRGB;
uint8_t* rgbBuf;
size_t bufSize;
bool ffmpeg_init()
{
avcodec_register_all();
pCodecCtx = avcodec_alloc_context3(NULL);
if (!pCodecCtx)
{
return false;
}
pCodecCtx->thread_count = 4;
pCodec = avcodec_find_decoder(AV_CODEC_ID_H264);
if (!pCodec || avcodec_open2(pCodecCtx, pCodec, NULL) < 0)
{
return false;
}
pCodecParserCtx = av_parser_init(AV_CODEC_ID_H264);
if (!pCodecParserCtx)
{
return false;
}
pFrameYUV = av_frame_alloc();
if (!pFrameYUV)
{
return false;
}
pFrameRGB = av_frame_alloc();
if (!pFrameRGB)
{
return false;
}
pSwsCtx = NULL;
pCodecCtx->flags2 |= AV_CODEC_FLAG2_SHOW_ALL;
}
#ifdef SDL2_INSTALLED
#include <SDL2/SDL.h>
void sdl_show_rgb(uint8_t *rgb24Buf, int width, int height) {
static SDL_Renderer* sdlRenderer = NULL;
static SDL_Texture* sdlTexture = NULL;
static SDL_Window *screen = NULL;
static int initFlag = 0;
if (initFlag == 0) {
initFlag = 1;
if(SDL_Init(SDL_INIT_VIDEO)) {
printf( "Could not initialize SDL - %s\n", SDL_GetError());
return;
}
//SDL 2.0 Support for multiple windows
screen = SDL_CreateWindow("camera_stream_node", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED,
100, 100,SDL_WINDOW_OPENGL|SDL_WINDOW_SHOWN|SDL_WINDOW_RESIZABLE);
if(!screen) {
printf("SDL: could not create window - exiting:%s\n",SDL_GetError());
return;
}
sdlRenderer = SDL_CreateRenderer(screen, -1, 0);
uint32_t pixformat = SDL_PIXELFORMAT_RGB24;
sdlTexture = SDL_CreateTexture(sdlRenderer,pixformat, SDL_TEXTUREACCESS_STREAMING, width, height);
}
if (!sdlRenderer || !sdlTexture || !screen) return;
SDL_SetWindowSize(screen, width, height);
SDL_Event event;
event.type = (SDL_USEREVENT + 1);
SDL_PushEvent(&event);
if (SDL_WaitEventTimeout(&event, 5)) {
SDL_Rect sdlRect;
SDL_UpdateTexture(sdlTexture, NULL, rgb24Buf, width * 3);
sdlRect.x = 0;
sdlRect.y = 0;
sdlRect.w = width;
sdlRect.h = height;
SDL_RenderClear(sdlRenderer);
SDL_RenderCopy(sdlRenderer, sdlTexture, NULL, &sdlRect);
SDL_RenderPresent(sdlRenderer);
}
}
#endif
void show_rgb(CameraRGBImage img, char* name)
{
std::cout << "#### Got image from:\t" << std::string(name) << std::endl;
#ifdef SDL2_INSTALLED
sdl_show_rgb(img.rawData.data(), img.width, img.height);
#elif defined(OPEN_CV_INSTALLED)
cv::Mat mat(img.height, img.width, CV_8UC3, img.rawData.data(), img.width*3);
cvtColor(mat, mat, cv::COLOR_RGB2BGR);
imshow(name,mat);
cv::waitKey(1);
#endif
}
void decodeToDisplay(uint8_t *buf, int bufLen)
{
uint8_t* pData = buf;
int remainingLen = bufLen;
int processedLen = 0;
AVPacket pkt;
av_init_packet(&pkt);
while (remainingLen > 0)
{
processedLen = av_parser_parse2(pCodecParserCtx, pCodecCtx,
&pkt.data, &pkt.size,
pData, remainingLen,
AV_NOPTS_VALUE, AV_NOPTS_VALUE, AV_NOPTS_VALUE);
remainingLen -= processedLen;
pData += processedLen;
if (pkt.size > 0)
{
int gotPicture = 0;
avcodec_decode_video2(pCodecCtx, pFrameYUV, &gotPicture, &pkt);
if (!gotPicture)
{
//DSTATUS_PRIVATE("Got Frame, but no picture\n");
continue;
}
else
{
int w = pFrameYUV->width;
int h = pFrameYUV->height;
//DSTATUS_PRIVATE("Got picture! size=%dx%d\n", w, h);
if(NULL == pSwsCtx)
{
pSwsCtx = sws_getContext(w, h, pCodecCtx->pix_fmt,
w, h, AV_PIX_FMT_RGB24,
4, NULL, NULL, NULL);
}
if(NULL == rgbBuf)
{
bufSize = avpicture_get_size(AV_PIX_FMT_RGB24, w, h);
rgbBuf = (uint8_t*) av_malloc(bufSize);
avpicture_fill((AVPicture*)pFrameRGB, rgbBuf, AV_PIX_FMT_RGB24, w, h);
}
if(NULL != pSwsCtx && NULL != rgbBuf)
{
sws_scale(pSwsCtx,
(uint8_t const *const *) pFrameYUV->data, pFrameYUV->linesize, 0, pFrameYUV->height,
pFrameRGB->data, pFrameRGB->linesize);
pFrameRGB->height = h;
pFrameRGB->width = w;
#ifdef SDL2_INSTALLED
sdl_show_rgb(pFrameRGB->data[0], pFrameRGB->width, pFrameRGB->height);
#elif defined(OPEN_CV_INSTALLED)
cv::Mat mat(pFrameRGB->height, pFrameRGB->width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->width * 3);
cv::cvtColor(mat, mat, cv::COLOR_RGB2BGR);
cv::imshow("camera_stream_node", mat);
cv::waitKey(1);
#endif
}
}
}
}
av_free_packet(&pkt);
}
void fpvCameraStreamCallBack(const sensor_msgs::Image& msg)
{
CameraRGBImage img;
img.rawData = msg.data;
img.height = msg.height;
img.width = msg.width;
char Name[] = "FPV_CAM";
show_rgb(img, Name);
std::cout<<"height is"<<msg.height<<std::endl;
std::cout<<"width is"<<msg.width<<std::endl;
}
void mainCameraStreamCallBack(const sensor_msgs::Image& msg)
{
CameraRGBImage img;
img.rawData = msg.data;
img.height = msg.height;
img.width = msg.width;
char Name[] = "MAIN_CAM";
show_rgb(img, Name);
std::cout<<"height is"<<msg.height<<std::endl;
std::cout<<"width is"<<msg.width<<std::endl;
}
void cameraH264CallBack(const sensor_msgs::Image& msg)
{
decodeToDisplay((uint8_t *)&msg.data[0], msg.data.size());
}
//****************将视频发布出去****************
//ros::Publisher pub;
int indexPub =0;
image_transport::Publisher image_pub;
void ImageCallback(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC3);
//cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);
cv::Mat cvImg = cv_ptr->image;
cv::Mat cvCompImg;
cv::resize(cvImg,cvCompImg,cv::Size(1080,720));
sensor_msgs::ImagePtr compImg = cv_bridge::CvImage(std_msgs::Header(),"rgb8",cvCompImg).toImageMsg();
//****************设置消息发布频率为5，默认频率为30？
if(indexPub++%6==0)
{
image_pub.publish(compImg);
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "camera_stream_node");
ros::NodeHandle nh;
/*! H264 flow init and H264 decoder init */
auto setup_camera_h264_client = nh.serviceClient<dji_osdk_ros::SetupCameraH264>("setup_camera_h264");
auto fpv_camera_h264_sub = nh.subscribe("dji_osdk_ros/camera_h264_stream", 10, cameraH264CallBack);
dji_osdk_ros::SetupCameraH264 setupCameraH264_;
ffmpeg_init();
/*! RGB flow init */
auto setup_camera_stream_client = nh.serviceClient<dji_osdk_ros::SetupCameraStream>("setup_camera_stream");
auto fpv_camera_stream_sub = nh.subscribe("dji_osdk_ros/fpv_camera_images", 10, fpvCameraStreamCallBack);
auto main_camera_stream_sub = nh.subscribe("c", 10, mainCameraStreamCallBack);
dji_osdk_ros::SetupCameraStream setupCameraStream_;
//****************发布消息
image_transport::ImageTransport it(nh);
image_transport::Subscriber image_sub;
image_sub = it.subscribe("/dji_osdk_ros/main_camera_images",5,ImageCallback);
image_pub = it.advertise("/image_compressed",5);
#ifndef SDL2_INSTALLED
std::cout
<< "--Recommandation : It is found that using \"cv::imshow\" will cause more CPU resources and more processing "
"time. Using SDL to display images can improve this situation. At present, SDL display is supported in this "
"node, which can be used by installing SDL2 library and recompiling. \n"
<< "--Install SDL2 library in shell : \"sudo apt-get install libsdl2-dev\"."
<< std::endl;
#endif
#ifdef SDL2_INSTALLED
std::cout << "Using SDL2 lib to display the images." << std::endl;
#elif defined(OPEN_CV_INSTALLED)
std::cout << "Using Opencv to display the images." << std::endl;
#endif
// char inputChar = 0;
// std::cout << std::endl;
// std::cout
// << "| Input 'm' or 'f' to view the camera stream from the RGB flow |\n"
// "| of the topic \"dji_osdk_ros/fpv_camera_images\" and |\n"
// "| \"dji_osdk_ros/main_camera_images\" : |"
// << std::endl
// << "| [m] Start to display main camera stream |"
// << std::endl
// << "| [f] Start to display fpv stream |"
// << std::endl;
// std::cout
// << "| |"
// << std::endl;
// std::cout
// << "| Input 'a' ~ 'b' to view the camera stream decoded by the h264|\n"
// "| flow from the topic \"dji_osdk_ros/camera_h264_stream\" : |"
// << std::endl
// << "| [a] Start to display FPV stream sample |"
// << std::endl
// << "| [b] Start to display main camera stream sample |"
// << std::endl
// << "| [c] Start to display vice camera stream sample |"
// << std::endl
// << "| [d] Start to display top camera stream sample |"
// << std::endl;
//std::cin >> inputChar;
//****************原先代码中是输入m，启动以下功能，现在，取消手动输入，直接运行以下功能
setupCameraStream_.request.cameraType = setupCameraStream_.request.MAIN_CAM;
setupCameraStream_.request.start = 1;
setup_camera_stream_client.call(setupCameraStream_);
// switch (inputChar)
// {
// case 'f':
// {
// setupCameraStream_.request.cameraType = setupCameraStream_.request.FPV_CAM;
// setupCameraStream_.request.start = 1;
// setup_camera_stream_client.call(setupCameraStream_);
// break;
// }
// case 'm':
// {
// setupCameraStream_.request.cameraType = setupCameraStream_.request.MAIN_CAM;
// setupCameraStream_.request.start = 1;
// setup_camera_stream_client.call(setupCameraStream_);
// break;
// }
// case 'a':
// {
// setupCameraH264_.request.request_view = setupCameraH264_.request.FPV_CAMERA;
// setupCameraH264_.request.start = 1;
// setup_camera_h264_client.call(setupCameraH264_);
// break;
// }
// case 'b':
// {
// setupCameraH264_.request.request_view = setupCameraH264_.request.MAIN_CAMERA;
// setupCameraH264_.request.start = 1;
// setup_camera_h264_client.call(setupCameraH264_);
// break;
// }
// case 'c':
// {
// setupCameraH264_.request.request_view = setupCameraH264_.request.VICE_CAMERA;
// setupCameraH264_.request.start = 1;
// setup_camera_h264_client.call(setupCameraH264_);
// break;
// }
// case 'd':
// {
// setupCameraH264_.request.request_view = setupCameraH264_.request.TOP_CAMERA;
// setupCameraH264_.request.start = 1;
// setup_camera_h264_client.call(setupCameraH264_);
// break;
// }
// }
ros::AsyncSpinner spinner(1);
spinner.start();
ros::Duration(10000).sleep(); //****************这个默认是20，即程序运行20S后，会进入睡眠程序
// switch (inputChar)
// {
// case 'f':
// {
// setupCameraStream_.request.cameraType = setupCameraStream_.request.FPV_CAM;
// setupCameraStream_.request.start = 0;
// setup_camera_stream_client.call(setupCameraStream_);
// break;
// }
// case 'm':
// {
// setupCameraStream_.request.cameraType = setupCameraStream_.request.MAIN_CAM;
// setupCameraStream_.request.start = 0;
// setup_camera_stream_client.call(setupCameraStream_);
// break;
// }
// case 'a':
// {
// setupCameraH264_.request.request_view = setupCameraH264_.request.FPV_CAMERA;
// setupCameraH264_.request.start = 0;
// setup_camera_h264_client.call(setupCameraH264_);
// break;
// }
// case 'b':
// {
// setupCameraH264_.request.request_view = setupCameraH264_.request.MAIN_CAMERA;
// setupCameraH264_.request.start = 0;
// setup_camera_h264_client.call(setupCameraH264_);
// break;
// }
// case 'c':
// {
// setupCameraH264_.request.request_view = setupCameraH264_.request.VICE_CAMERA;
// setupCameraH264_.request.start = 0;
// setup_camera_h264_client.call(setupCameraH264_);
// break;
// }
// case 'd':
// {
// setupCameraH264_.request.request_view = setupCameraH264_.request.TOP_CAMERA;
// setupCameraH264_.request.start = 0;
// setup_camera_h264_client.call(setupCameraH264_);
// break;
// }
// }
ROS_INFO_STREAM("Finished. Press CTRL-C to terminate the node");
ros::waitForShutdown();
return 0;
}
【参考链接】：
扫一扫
822
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
