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;
}