ROS之topic

最新推荐文章于 2024-08-12 00:30:00 发布

felicx温水煮青蛙

最新推荐文章于 2024-08-12 00:30:00 发布

阅读量1k

点赞数 1

分类专栏： ROS

本文链接：https://blog.csdn.net/qq_37427972/article/details/99090681

版权

ROS 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

一、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：可参考这篇：https://www.cnblogs.com/li-yao7758258/p/6651326.html

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