之前做的项目需要用到ROS,在这方面几乎小白的我用了几天时间入了下门,又用了几天时间调bug,最终基本上实现了如下需求功能:
(1)订阅话题侦听外界的控制信息task_type,并初始化task_type=0;
(2)当task_type=1时,首先需要抓取彩色图和深度图各一张作为算法输入,然后运行算法,算法的输出结果包括一个pos数组和一张可视化图片;
(3)将算法的结果通过话题发布出去。
首先定义三个回调函数task_callback、color_callback、depth_callback,分别用来侦听控制信息、采集彩色图像、采集深度图像。在while循环中不断侦听task_type,如果未收到外界控制信息则task_type=0,程序处于等待状态;当收到外界控制信息1时,task_type=1,采集图像、运行算法并发布结果;当收到外界控制信息除0和1之外的值时,关闭所有话题结束循环退出程序。下面给出程序的main.cpp,算法的具体实现涉及项目内容不方便给出。该程序的实现思路比较简单,可以实现上述功能,但对于一些异常情况的判断还有待完善。
main.cpp
#include <stdlib.h>
#include <string>
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/Image.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include "algo.h"
int task_type = 0;
cv::Mat rgb, depth, view;
float pos[24];
void task_callback(const std_msgs::Int32::ConstPtr& topic_msg)
{
ROS_INFO("subscribing information: %d", topic_msg->data);
task_type = topic_msg->data;
}
void color_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
if(task_type == 1)
{
ros::Time time = img_msg->header.stamp;
std::string rgb_name = std::to_string(time.sec) + "_rgb.png";
ROS_INFO("get image: %s", rgb_name.c_str());
rgb = cv_bridge::toCvCopy(img_msg, "8UC3")->image;
cv::cvtColor(rgb, rgb, cv::COLOR_BGR2RGB);
cv::imwrite(rgb_name, rgb);
}
}
void depth_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
if(task_type == 1)
{
ros::Time time = img_msg->header.stamp;
std::string depth_name = std::to_string(time.sec) + "_depth.png";
ROS_INFO("get image: %s", depth_name.c_str());
depth = cv_bridge::toCvCopy(img_msg, "16UC1")->image;
cv::imwrite(depth_name, depth);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "algo");
ros::NodeHandle task_nh, color_nh, depth_nh, algo_nh, view_nh;
ros::Subscriber sub_task = task_nh.subscribe("/control", 1, task_callback);
ros::Subscriber sub_rgb = color_nh.subscribe("/color", 1, color_callback);
ros::Subscriber sub_depth = depth_nh.subscribe("/depth", 1, depth_callback);
ros::Publisher pub_pose = algo_nh.advertise<std_msgs::Float32MultiArray>("/pose", 1);
ros::Publisher pub_view = view_nh.advertise<sensor_msgs::Image>("/view", 1);
ros::Rate r(1);
while(ros::ok()) //不写ros::ok()会死循环
{
std::cout << "task_type: " << task_type << std::endl;
if(task_type == 0)
{
ros::spinOnce();
r.sleep();
}
else if(task_type == 1)
{
ros::spinOnce();
r.sleep();
Algo *algo = new Algo(rgb, depth);
algo->process();
memcpy(pos, &(algo->result_algo)[0], algo->result_algo.size() * sizeof(float));
view = algo->view.clone();
std_msgs::Float32MultiArray msg;
for(int i=0;i<algo->result_algo.size();++i)
msg.data.push_back(pos[i]);
pub_pose.publish(msg);
sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", view).toImageMsg();
pub_view.publish(img_msg);
task_type = 0;
}
else
{
sub_task.shutdown();
sub_rgb.shutdown();
sub_depth.shutdown();
pub_pose.shutdown();
pub_view.shutdown();
break;
}
}
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(test)
set(SRC_LIST main.cpp algo.cpp)
set(Boost_INCLUDE_DIR /usr/include)
set(Boost_LIBRARY_DIR /usr/lib/aarch64-linux-gnu)
find_package(OpenCV REQUIRED)
find_package(PCL REQUIRED)
find_package(catkin REQUIRED COMPONENTS roscpp)
include_directories(${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} algo.h)
link_directories(${OpenCV_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS})
add_definitions(${OpenCV_DEFINITIONS} ${PCL_DEFINITIONS})
add_executable(test ${SRC_LIST})
target_link_libraries(test ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${catkin_LIBRARIES})
发布控制信息可以通过rostopic pub命令实现,查看发布的消息可以通过rostopic echo命令实现,发布的图片可以通过rqt_image_view查看。