ROS 之 topic 发布

Ros使用topic发布array：

C++实现publish这里只需要数据data数据

#include "ros/ros.h"
#include "std_msgs/Float32MultiArray.h"
int main(int argc, char **argv)
{
    ros::init(argc, argv, "Array_pub");
    ros::NodeHandle nh;

    ros::Publisher chatter_pub = nh.advertise<std_msgs::Float32MultiArray>("chatter", 1000);

    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        std_msgs::Float32MultiArray msg;
        msg.data.push_back(1.0);//自己写的，可行
        msg.data.push_back(2.0);
        msg.data.push_back(3.0);
        msg.data.push_back(4.0);

        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
// 订阅
#include "ros/ros.h"
#include "std_msgs/Float32MultiArray.h"

void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
    ROS_INFO("I heard: [%f],[%f],[%f],[%f]", msg->data.at(0),msg->data.at(1),msg->data.at(2),msg->data.at(3));
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "Array_sub");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("chatter", 1000, chatterCallback);
    ros::spin();
    return 0;
}

python实现

#! /usr/bin/python
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import Float32MultiArray
def talker():
    pub_p = rospy.Publisher('lefttop_point', Float32MultiArray, queue_size=1)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
    array = [521,1314]
    left_top = Float32MultiArray(data=array)
        #也可以采用下面的形式赋值
        #left_top = Float32MultiArray()
        #left_top.data = [521,1314]
        #left_top.label = 'love'
        rospy.loginfo(left_top)
    pub_p.publish(left_top)
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

// Publish.cpp
#include <stdio.h>
#include <stdlib.h>

#include "ros/ros.h"

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"

#include "std_msgs/Int32MultiArray.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "arrayPublisher");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<std_msgs::Int32MultiArray>("array", 100);
    while (ros::ok())
    {
        std_msgs::Int32MultiArray array;
        //Clear array
        array.data.clear();
        //for loop, pushing data in the size of the array
        for (int i = 0; i < 90; i++)
        {
            //assign array a random number between 0 and 255.
            array.data.push_back(rand() % 255);
        }
        //Publish array
        pub.publish(array);
        //Let the world know
        ROS_INFO("I published something!");
        //Do this.
        ros::spinOnce();
        //Added a delay so not to spam
        sleep(2);
    }

}

// Subscribe.cpp
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <iostream>

#include "ros/ros.h"

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Int32MultiArray.h"

int Arr[90];
void arrayCallback(const std_msgs::Int32MultiArray::ConstPtr& array);

int main(int argc, char **argv)
{
    ros::init(argc, argv, "arraySubscriber");
    ros::NodeHandle n;
    ros::Subscriber sub3 = n.subscribe("array", 100, arrayCallback);
    ros::spinOnce();

    for(j = 1; j < 90; j++)
    {
        printf("%d, ", Arr[j]);
    }

    printf("\n");
    return 0;
}

void arrayCallback(const std_msgs::Int32MultiArray::ConstPtr& array)
{
    int i = 0;
    // print all the remaining numbers
    for(std::vector<int>::const_iterator it = array->data.begin(); it != array->data.end(); ++it)
    {
        Arr[i] = *it;
        i++;
    }

    return;
}

std_msgs、geometry_msgs

这些直接搜ros wiki，都有用法的。

Ros使用topic发布LaserScan和PointCloud：

消息类型：sensor_msgs/LaserScan和 sensor_msgs/PointCloud跟其他的消息一样，包括tf帧和与时间相关的信息。为了标准化发送这些信息，消息类型Header被用于所有此类消息的一个字段。 Header类型：Header包括是哪个字段。字段seq对应一个标识符，随着消息被发布，它会自动增加。字段stamp存储与数据相关联的时间信息。以激光扫描为例，stamp可能对应每次扫描开始的时间。字段frame_id存储与数据相关联的tf帧信息。以激光扫描为例，它将是激光数据所在帧。

sensor_msgs/LaserScan Message：

# 这里有啥就填啥,就相当一个结构体X，然后(X.参数)即可
# 测量的激光扫描角度，逆时针为正
# 设备坐标帧的0度面向前（沿着X轴方向）
#
Header header
float32 angle_min        # scan的开始角度 [弧度]
float32 angle_max        # scan的结束角度 [弧度]
float32 angle_increment  # 测量的角度间的距离 [弧度]
float32 time_increment   # 测量间的时间 [秒]
float32 scan_time        # 扫描间的时间 [秒]
float32 range_min        # 最小的测量距离 [米]
float32 range_max        # 最大的测量距离 [米]
float32[] ranges         # 测量的距离数据 [米] (注意: 值 < range_min 或 > range_max 应当被丢弃)
float32[] intensities    # 强度数据 [device-specific units]

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "laser_scan_publisher");

  ros::NodeHandle n;
  ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

  unsigned int num_readings = 100;
  double laser_frequency = 40;
  double ranges[num_readings];
  double intensities[num_readings];

  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    //generate some fake data for our laser scan
    //设置消息的长度，便于填充一些虚拟数据。真正的应用程序将从他们的激光扫描仪中获取数据
    for(unsigned int i = 0; i < num_readings; ++i){
      ranges[i] = count;
      intensities[i] = 100 + count;
    }
    ros::Time scan_time = ros::Time::now();

    //populate the LaserScan message
    sensor_msgs::LaserScan scan;
    scan.header.stamp = scan_time;
    scan.header.frame_id = "laser_frame";
    scan.angle_min = -1.57;
    scan.angle_max = 1.57;
    scan.angle_increment = 3.14 / num_readings;
    scan.time_increment = (1 / laser_frequency) / (num_readings);
    scan.range_min = 0.0;
    scan.range_max = 100.0;

    scan.ranges.resize(num_readings);   //使用resize设定激光点的多少
    scan.intensities.resize(num_readings);
    //用每秒增加1的值填充虚拟激光数据
    for(unsigned int i = 0; i < num_readings; ++i){
      scan.ranges[i] = ranges[i];
      scan.intensities[i] = intensities[i];
    }

    scan_pub.publish(scan);
    ++count;
    r.sleep();
  }
}

sensor_msgs/PointCloud Message：可参考这篇

#This message holds a collection of 3d points, plus optional additional information about each point.
#Each Point32 should be interpreted as a 3d point in the frame given in the header

Header header
geometry_msgs/Point32[] points  #Array of 3d points
ChannelFloat32[] channels       #Each channel should have the same number of elements as points array, and the data in each channel should correspond 1:1 with each point

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "point_cloud_publisher");

  ros::NodeHandle n;
  ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

  unsigned int num_points = 100;

  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "sensor_frame";//填充 PointCloud 消息的头：frame 和 timestamp．

    cloud.points.resize(num_points);//设置点云的数量．

    //增加信道 "intensity" 并设置其大小，使与点云数量相匹配．
    cloud.channels.resize(1);
    cloud.channels[0].name = "intensities";
    cloud.channels[0].values.resize(num_points);

    //使用虚拟数据填充 PointCloud 消息．同时，使用虚拟数据填充 intensity 信道．
    for(unsigned int i = 0; i < num_points; ++i){
      cloud.points[i].x = 1 + count;
      cloud.points[i].y = 2 + count;
      cloud.points[i].z = 3 + count;
      cloud.channels[0].values[i] = 100 + count;
    }

    cloud_pub.publish(cloud);
    ++count;
    r.sleep();
  }
}

ros订阅velodyne激光的点云数据

import numpy as np
import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import scipy.misc
import os
def point_cloud_2_birdseye(points):
    x_points = points[:, 0]
    y_points = points[:, 1]
    z_points = points[:, 2]

    f_filt = np.logical_and((x_points > -50), (x_points < 50))
    # logical_and（逻辑与）
    s_filt = np.logical_and((y_points > -50), (y_points < 50))
    filter = np.logical_and(f_filt, s_filt)
    indices = np.argwhere(filter)	# 筛选符合范围的points
    # 返回符合filter条件的位置索引，即第几个位置

    x_points = x_points[indices]
    y_points = y_points[indices]
    z_points = z_points[indices]

    x_img = (-y_points*10).astype(np.int32)+500
    # 转换数组的数据类型
    # 点云数据通常是浮点数，而图像数据通常是整数，所以要float映射到int
    y_img = (-x_points *10).astype(np.int32)+500

    pixel_values = np.clip(z_points,-2,2)
    # numpy.clip(a, a_min, a_max, out=None)
    # 将数组中的元素限制在-2和2之间，大于2的就使得它等于2，小于-2,的就使得它等于-2
    pixel_values = ((pixel_values +2) / 4.0) * 255

    im = np.zeros([1001, 1001], dtype=np.uint8)
    im[y_img, x_img] = pixel_values
    return im

def callback(lidar):
    lidar = pc2.read_points(lidar)
    # 函数 point_cloud2.read_points(data, field_names=("x", "y", "z"), skip_nans=True)
    # 这个函数返回值是一个generator（python中的生成器，属于Iterator迭代器的一种）
    points = np.array(list(lidar))
    # 如果需要一次获得全部点，可以用list()转换为列表
    im = point_cloud_2_birdseye(points)
    scipy.misc.imsave('./lidar.png', im)	# 将数组保存成图像
    os._exit(0)		# python无错误退出程序

def cloud_subscribe():
    rospy.init_node('cloud_subscribe_node')
    rospy.Subscriber("/velodyne_points", PointCloud2, callback)
    rospy.spin()
cloud_subscribe()

Ros将回调函数写成类的形式：

wiki 上介绍，在ROS中，想在回调函数中发布消息，有两个思路：
1、把函数写成类的形式，把需要的一些变量在类中声明为全局变量。【推荐，模块化好】
2、在函数中，把回调函数需要调用的变量声明为全局变量。也可以解决这个问题。【不好，不符合面向对象的风格】下面的例子是在同一个节点中实现订阅一个消息，然后在该消息的回调函数中处理一下这些数据后再发布到另一个topic上。

#include <ros/ros.h>

class SubscribeAndPublish
{
    public:
    SubscribeAndPublish()
    {
        //Topic you want to publish
        pub_ = n_.advertise<PUBLISHED_MESSAGE_TYPE>("/published_topic", 1);
        //PUBLISHED_MESSAGE_TYPE例如std_msgs::String

        //Topic you want to subscribe
        sub_ = n_.subscribe("/subscribed_topic", 1, &SubscribeAndPublish::callback, this);  //注意这里，和wiki上不一样。&SubscribeAndPublish这个是类名
        //之所以用this，是因为第四个参数是一个指向【回调函数所在对象】的指针，官方文档例子里把sub定义在了类外面，我们把sub定义在了类的构造函数里面，所以this就是在实例化对象的时候指向对象的指针。（关于this：当调用成员函数a.volume 时，编译系统就把对象a的起始地址赋给this指针；构造函数：建立对象时自动执行。结合两者，在本例中建立类对象时，自动生成指向本对象的指针。）
    }

    //SUBSCRIBED_MESSAGE_TYPE例如std_msgs::String,记得&要保留
    void callback(const SUBSCRIBED_MESSAGE_TYPE& input)
    {
        PUBLISHED_MESSAGE_TYPE output;
        //.... do something with the input and generate the output...
        //output = ...
        pub_.publish(output);
    }

    private:
    ros::NodeHandle n_;
    ros::Publisher pub_;
    ros::Subscriber sub_;

};//End of class SubscribeAndPublish

int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "subscribe_and_publish");

    //Create an object of class SubscribeAndPublish that will take care of everything
    SubscribeAndPublish SAPObject;

    ros::spin();

    return 0;
}