ros自定义动态消息发布(vector)
原本我要节点发布一张图片,并在在rviz上显示, 图片是一副背景图再加一些点. 如果在运算的主程序内,每次都要发布整张图片,代价有点高.
所以决定将相应要绘制的点发布出来,再开辟一个node进行绘图处理与图片发布. ===> 这些点的个数是动态的,因此需要进行动态数组msg的发布
1. 写/msg/*****.msg文件
ROS Message types.
#landmark.msg uint8 id float32 x float32 y float32 theta
#odom.msg float32 x float32 y float32 theta
#robot.msg float32 x float32 y float32 theta
#systemState.msg odom odom robot robot_state landmark[] landmark
2. CMakeLists.txt 与 package.xml 书写
将msg文件编译生成相应的c++ 可以识别的头文件 详见: 创建一个 msg
#CMakeLists.txt find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation) add_message_files( FILES odom.msg robot.msg landmark.msg systemState.msg ) generate_messages( DEPENDENCIES std_msgs)
# package.xml <build_depend>message_generation</build_depend> <run_depend>message_runtime</run_depend>
3. node设计
说明: .h文件一般生成在 ~/catkin_ws/devel/include/你的包名/***.h
我的在 /home/yhzhao/gmapping_ws/devel/include/cv_slam/**.h
头文件添加
#include <cv_slam/odom.h> #include <cv_slam/landmark.h> #include <cv_slam/robot.h> #include <cv_slam/systemState.h> ros::Publisher systemStates_;
发布节点部分程序
systemStates_ = ar_handle.advertise<cv_slam::systemState>("systemStates", 10);
void QrSlam::pubSystemStates()
{
cv_slam::systemState sys_state;
cv_slam::odom odom;
odom.x = robot_odom_.x;
odom.y = robot_odom_.y;
odom.theta = robot_odom_.theta;
cv_slam::robot robot;
robot.x = miu_state_.at<float>(0);
robot.y = miu_state_.at<float>(1);
robot.theta = miu_state_.at<float>(2);
vector<int> landmarkIDs = getLandmarksIndex();
for(int i=0; i<landmarkIDs.size(); i++)
{
cv_slam::landmark landmark;
landmark.x = miu_state_.at<float>(3*i +3);
landmark.y = miu_state_.at<float>(3*i +4);
landmark.theta = miu_state_.at<float>(3*i +5);
landmark.id = landmarkIDs.at(i);
sys_state.landmark.push_back(landmark);
}
sys_state.odom = odom;
sys_state.robot_state = robot;
systemStates_.publish(sys_state);
}
接收节点的设计: 全部代码见尾部.
#include <cv_slam/odom.h>
#include <cv_slam/landmark.h>
#include <cv_slam/robot.h>
#include <cv_slam/systemState.h>
ros::Publisher image_pub_ ;
ros::Publisher state_pub_ ;
Mat map_;
Mat view_;
Mat state_img_;
<pre name="code" class="cpp">void chatterCallback(const cv_slam::systemStateConstPtr& msg)
{
cv_slam::odom odom = msg->odom;
cv_slam::robot robot = msg->robot_state;
vector<cv_slam::landmark> landmarks = msg->landmark;
showOdom(map_, odom, CV_RGB(0,0,0)) ;
showRobot(map_, robot, CV_RGB(0,0,255));
drawCoordinate(map_);
map_.copyTo( view_ ); //避免历史重影 landmark
showLandmarks(view_, landmarks, CV_RGB(0,0,0));
resize(view_, view_, map_size_);
cv_bridge::CvImage cvi;
sensor_msgs::Image ros_img;
ros::Time time=ros::Time::now();
cvi.header.stamp = time;
cvi.header.frame_id = "image";
cvi.encoding = "bgr8";
cvi.image = view_;//Mat
cvi.toImageMsg(ros_img);
image_pub_.publish(cvi.toImageMsg());
Mat state_show;
state_img_.copyTo( state_show ); //避免历史重影 landmark
showOdom(state_show, odom, CV_RGB(0,0,0),50,50);
showRobot(state_show, robot, CV_RGB(0,0,0),300,50);
cv_bridge::CvImage cvi_state;
sensor_msgs::Image state_img;
ros::Time time2=ros::Time::now();
cvi_state.header.stamp = time2;
cvi_state.header.frame_id = "image";
cvi_state.encoding = "bgr8";
cvi_state.image = state_show; //Mat
cvi_state.toImageMsg(state_img);
state_pub_.publish(cvi_state.toImageMsg());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "view");
ros::NodeHandle n;
map_ = cv::imread("./OutPut/map/map.png", CV_LOAD_IMAGE_COLOR); //for display
map_size_.width = map_.cols * map_scale_;
map_size_.height = map_.rows * map_scale_;
resize(map_, state_img_, Size(640,480));
ros::Subscriber sub = n.subscribe("/systemStates", 10, chatterCallback);
image_pub_ = n.advertise<sensor_msgs::Image>("/view", 10);
state_pub_ = n.advertise<sensor_msgs::Image>("/state_img", 10);
ros::spin();
return 0;
}
参考:
python 版 : http://answers.ros.org/question/60614/how-to-publish-a-vector-of-unknown-length-of-structs-in-ros/
c++ 版: http://stackoverflow.com/questions/15992990/how-would-you-publish-a-message-in-ros-of-a-vector-of-structs
CreatingMsgAndSrv : http://wiki.ros.org/cn/ROS/Tutorials/CreatingMsgAndSrv
=================================================
//接收节点代码
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
//#include "../image_convert/image_converter.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <cv_slam/odom.h>
#include <cv_slam/landmark.h>
#include <cv_slam/robot.h>
#include <cv_slam/systemState.h>
using namespace std;
using namespace cv;
void showOdom(cv::Mat& map, const cv_slam::odom rob_odom, const Scalar rgb);
void showRobot(cv::Mat& map, const cv_slam::robot robot, const Scalar rgb);
void drawCoordinate(cv::Mat& mat);
std::string int2str(int num);
void showLandmarks(cv::Mat& map, const vector<cv_slam::landmark> landmarks, const Scalar rgb);
void chatterCallback(const cv_slam::systemStateConstPtr& msg);
std::string float2str(float num);
void showOdom(Mat image, const cv_slam::odom rob_odom, const Scalar rgb, const int x_coordinate, const int y_coordinate);
void showRobot(Mat image, const cv_slam::robot rob_odom, const Scalar rgb, const int x_coordinate, const int y_coordinate);
ros::Publisher image_pub_ ;
ros::Publisher state_pub_ ;
Mat map_;
Mat view_;
Mat state_img_;
const int map_width_ = 2000;// = 150 ;
const int map_height_ = 2000 ;// = 100 ;
const int map_base_x_ = 1000;// = 150 ;
const int map_base_y_ = 1000 ;// = 100 ;
const float map_scale_ = 0.25; //缩放倍数
Size map_size_; //目标图像尺寸
int main(int argc, char **argv)
{
ros::init(argc, argv, "view");
ros::NodeHandle n;
map_ = cv::imread("./OutPut/map/map.png", CV_LOAD_IMAGE_COLOR); //for display
map_size_.width = map_.cols * map_scale_;
map_size_.height = map_.rows * map_scale_;
resize(map_, state_img_, Size(640,480));
ros::Subscriber sub = n.subscribe("/systemStates", 10, chatterCallback);
image_pub_ = n.advertise<sensor_msgs::Image>("/view", 10);
state_pub_ = n.advertise<sensor_msgs::Image>("/state_img", 10);
ros::spin();
return 0;
}
void chatterCallback(const cv_slam::systemStateConstPtr& msg)
{
cv_slam::odom odom = msg->odom;
cv_slam::robot robot = msg->robot_state;
vector<cv_slam::landmark> landmarks = msg->landmark;
showOdom(map_, odom, CV_RGB(0,0,0)) ;
showRobot(map_, robot, CV_RGB(0,0,255));
drawCoordinate(map_);
map_.copyTo( view_ ); //避免历史重影 landmark
showLandmarks(view_, landmarks, CV_RGB(0,0,0));
resize(view_, view_, map_size_);
cv_bridge::CvImage cvi;
sensor_msgs::Image ros_img;
ros::Time time=ros::Time::now();
cvi.header.stamp = time;
cvi.header.frame_id = "image";
cvi.encoding = "bgr8";
cvi.image = view_;//Mat
cvi.toImageMsg(ros_img);
image_pub_.publish(cvi.toImageMsg());
Mat state_show;
state_img_.copyTo( state_show ); //避免历史重影 landmark
showOdom(state_show, odom, CV_RGB(0,0,0),50,50);
showRobot(state_show, robot, CV_RGB(0,0,0),300,50);
cv_bridge::CvImage cvi_state;
sensor_msgs::Image state_img;
ros::Time time2=ros::Time::now();
cvi_state.header.stamp = time2;
cvi_state.header.frame_id = "image";
cvi_state.encoding = "bgr8";
cvi_state.image = state_show; //Mat
cvi_state.toImageMsg(state_img);
state_pub_.publish(cvi_state.toImageMsg());
}
void showOdom(cv::Mat& map, const cv_slam::odom rob_odom, const Scalar rgb)
{
Point3d robot_pose;
robot_pose.x = rob_odom.x;
robot_pose.y = rob_odom.y;
robot_pose.z = rob_odom.theta;
const int ROBOT_DEFAULT_RADIUS = 2;
const int ROBOT_DEFAULT_ARROW_LEN = 10;
Point start, end;
start.x = robot_pose.x + map_base_x_;
start.y = robot_pose.y + map_base_y_;
int thickness = 2;
int lineType = 8;
end.x = start.x + ROBOT_DEFAULT_ARROW_LEN * cos(robot_pose.z);
end.y = start.y + ROBOT_DEFAULT_ARROW_LEN * sin(robot_pose.z); //display y convert ..
start.y = map.rows - start.y;
end.y = map.rows - start.y;
circle(map,start,ROBOT_DEFAULT_RADIUS,rgb,0,lineType );
//line( map,start,end,rgb,thickness,lineType );
}
void showRobot(cv::Mat& map, const cv_slam::robot robot, const Scalar rgb)
{
int temp_X = robot.x + map_base_x_;
int temp_Y = robot.y + map_base_y_;
temp_Y = map.rows - temp_Y ;
cv::circle(map,Point( temp_X,temp_Y),2,rgb,1); //绘制 robot
}
void drawCoordinate(cv::Mat& mat)
{
std::string text ="Y";
cv::putText(mat,text,Point(20,20),CV_FONT_HERSHEY_COMPLEX,1,CV_RGB(0,0,255));
cv::line(mat,cv::Point(1,1),cv::Point(1,mat.rows),CV_RGB(255,0,0),1,8);
text ="O";
cv::putText(mat,text,Point(20,mat.rows-20),CV_FONT_HERSHEY_COMPLEX,1,CV_RGB(0,0,255));
text ="X";
cv::putText(mat,text,Point(mat.cols-20,mat.rows-20),CV_FONT_HERSHEY_COMPLEX,1,CV_RGB(0,0,255));
cv::line(mat,cv::Point(1,mat.rows-1),cv::Point(mat.cols,mat.rows-1),CV_RGB(255,0,0),1,8);
}
void showLandmarks(cv::Mat& map, const vector<cv_slam::landmark> landmarks, const Scalar rgb)
{
for (int t = 0; t < landmarks.size(); t++)
{
cv_slam::landmark lanmark = landmarks.at(t);
float X= lanmark.x + map_base_x_;
float Y= lanmark.y + map_base_y_;
Y = map.rows - Y ;
cv::circle(map,Point( X,Y),2,rgb,2); //绘制mark位置
//cv::Mat landmark_cov = getCovMatrix(miu_state, 3+t*3, 4+t*3);
//cv::Point2f center(X,Y);
// drawEllipse(map, center, landmark_cov, CV_RGB(0, 150,0) );
std::string text = int2str(lanmark.id);
cv::putText(map,text,Point(X,Y+20),CV_FONT_HERSHEY_COMPLEX, 2, CV_RGB(255, 0,0) );
}
}
void showOdom(Mat image, const cv_slam::odom odom, const Scalar rgb, const int x_coordinate, const int y_coordinate)
{
const int ROBOT_DEFAULT_RADIUS = 2;
const int ROBOT_DEFAULT_ARROW_LEN = 30;
Point3d robot_pose;
robot_pose.x = odom.x;
robot_pose.y = odom.y;
robot_pose.z = odom.theta;
//采用地图固定下 初始坐标系调整就可以
Point start, end;
start.x = x_coordinate;
start.y = y_coordinate;
int thickness = 1;
int lineType = 8;
line( image,start,start+Point(500,0),CV_RGB(0,255,0),1,lineType ); // x轴
line( image,start,start+Point(0,500),CV_RGB(0,155,0),1,lineType ); // y轴
circle(image,start,ROBOT_DEFAULT_RADIUS,rgb,2,lineType );
end.x = start.x + ROBOT_DEFAULT_ARROW_LEN * cos(robot_pose.z); //放大5倍
end.y = start.y - ROBOT_DEFAULT_ARROW_LEN * sin(robot_pose.z); //display y convert
line( image,start,end,rgb,thickness,lineType );
// 标记坐标信息
std::string text_id ="x: "+ float2str( robot_pose.x );
cv::putText(image, text_id, Point(50,150), CV_FONT_HERSHEY_COMPLEX, 1, CV_RGB(0,0,255));
std::string text1 ="y: "+ float2str( robot_pose.y );
cv::putText(image,text1, Point(50,200), CV_FONT_HERSHEY_COMPLEX, 1, CV_RGB(0,0,255));
std::string text2 ="z: "+ float2str( robot_pose.z*180/ 3.14159 );
cv::putText(image, text2, Point(50,250), CV_FONT_HERSHEY_COMPLEX, 1, CV_RGB(0,0,255));
}
void showRobot(Mat image, const cv_slam::robot robot, const Scalar rgb, const int x_coordinate, const int y_coordinate)
{
const int ROBOT_DEFAULT_RADIUS = 2;
const int ROBOT_DEFAULT_ARROW_LEN = 30;
Point3d robot_pose;
robot_pose.x = robot.x;
robot_pose.y = robot.y;
robot_pose.z = robot.theta;
Point start, end;
start.x = x_coordinate;
start.y = y_coordinate;
int thickness = 1;
int lineType = 8;
line( image,start,start+Point(500,0),CV_RGB(0,255,0),1,lineType ); // x轴
line( image,start,start+Point(0,500),CV_RGB(0,155,0),1,lineType ); // y轴
circle(image,start,ROBOT_DEFAULT_RADIUS,rgb,2,lineType );
end.x = start.x + ROBOT_DEFAULT_ARROW_LEN * cos( robot_pose.z );
end.y = start.y - ROBOT_DEFAULT_ARROW_LEN * sin( robot_pose.z ); //display y convert
line( image,start,end,rgb,thickness,lineType );
// 标记坐标信息
std::string text_id ="x: "+ float2str( robot_pose.x );
cv::putText(image,text_id,Point(300,150),CV_FONT_HERSHEY_COMPLEX,1,CV_RGB(255,0,0));
std::string text1 ="y: "+ float2str( robot_pose.y );
cv::putText(image,text1,Point(300,200),CV_FONT_HERSHEY_COMPLEX,1,CV_RGB(255,0,0));
std::string text2 ="z: "+ float2str( robot_pose.z *180/ 3.14159);
cv::putText(image,text2,Point(300,250),CV_FONT_HERSHEY_COMPLEX,1,CV_RGB(255,0,0));
}
std::string int2str(int num)
{
std::stringstream ss;
ss << num;
std::string text = ss.str();
return text;
}
std::string float2str(float num)
{
std::stringstream ss;
ss << num;
std::string text = ss.str();
return text;
}