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


  • 1
    点赞
  • 18
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值