一、Ros使用topic發佈array：

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

2、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

3、ROS Publish/Subscribe Arrays Example

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幀信息。以激光掃描為例，它將是激光數據所在幀。 http://docs.ros.org/melodic/api/std_msgs/html/msg/Header.html

1、sensor_msgs/LaserScan Message：

http://docs.ros.org/melodic/api/sensor_msgs/html/msg/LaserScan.html

# 這裏有啥就填啥,就相當一個結構體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();
  }
}

 

 

 

2、sensor_msgs/PointCloud Message： 可參考這篇：

http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud.html

#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();
  }
}

 

 

3、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上介紹：http://wiki.ros.org/roscpp/Overview/Publishers%20and%20Subscribers (2.3.2) 在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;  
}