ROS之topic
程序员文章站
2022-07-12 09:48:17
...
一、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:
pass3、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;
}
上一篇： 54.螺旋矩阵