python写一个ros2订阅者脚本订阅激光雷达话题数据 ros订阅topic

转载

西洋无悔 2023-12-30 10:49:23

文章标签 时间间隔配置文件 ide 文章分类 Python 后端开发

Cartographer_ros主要实现Topics的订阅与发布。订阅是指从IMU，激光雷达，里程计取数据，然后传给Cartographer库。发布是指从Cartographer拿处理的结果，然后发布给ROS，然后可以在rivz上显示。

1. cartographer_ros/docs/source/ros_api.rst

此文件描述了Cartographer_ros订阅与发布了哪些topics及服务。

2.运行 cartographer_ros

roslaunch cartographer_ros demo_revo_lds.launch

3.revo_lds.lua

此配置文件，描述了发布哪些Topics及名称。在源码中。订阅的Topic在源码中通过ratio of the pulse（脉冲的比率）的方式来定时调用。发布是通过定时器的方式，定时发布。具体值，可以通过此配置文件配置。

1)定时器方式

wall_timers_.push_back(node_handle_.createWallTimer(

::ros::WallDuration(node_options_.submap_publish_period_sec),

&Node::PublishSubmapList, this));

2)ratio方式

void Node::HandlePointCloud2Message(

const int trajectory_id, const std::string& sensor_id,

const sensor_msgs::PointCloud2::ConstPtr& msg) {

absl::MutexLock lock(&mutex_);

if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {

return;

}

map_builder_bridge_.sensor_bridge(trajectory_id)

->HandlePointCloud2Message(sensor_id, msg);

}

4. cartographer_node

Cartographer_ros中的一个重要节点

1.请先查看：cartographer_ros/docs/source/ros_api.rst 文档

＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝

Subscribed Topics：

scan (`sensor_msgs/LaserScan`_)

echoes (`sensor_msgs/MultiEchoLaserScan`_)

points2 (`sensor_msgs/PointCloud2`_)

imu (`sensor_msgs/Imu`_)

odom (`nav_msgs/Odometry`_)

Published Topics：

submap_list (`cartographer_ros_msgs/SubmapList`_)

Services：

submap_query (`cartographer_ros_msgs/SubmapQuery`_)

start_trajectory (`cartographer_ros_msgs/StartTrajectory`_)

trajectory_query (`cartographer_ros_msgs/TrajectoryQuery`_)

finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_)

write_state (`cartographer_ros_msgs/WriteState`_)

get_trajectory_states (`cartographer_ros_msgs/GetTrajectoryStates`_)

read_metrics (`cartographer_ros_msgs/ReadMetrics`_)

＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝

2. 运行cartographer_ros

roslaunch cartographer_ros demo_revo_lds.launch

================================

3.revo_lds.lua

demo_revo_lds.launch 指定加载revo_lds.lua 配置文件。指定topic的名称，发布的Topic的时间间隔。

map_frame = "map",

tracking_frame = "scan",

published_frame = "scan",

odom_frame = "odom",

provide_odom_frame = false,

use_odometry = false,

use_nav_sat = false,

use_landmarks = false,

publish_frame_projected_to_2d=false,

num_laser_scans = 1,

num_multi_echo_laser_scans = 0,

num_subdivisions_per_laser_scan = 1,

num_point_clouds = 0,

lookup_transform_timeout_sec = 0.2,

submap_publish_period_sec = 0.3,

pose_publish_period_sec = 5e-3,

trajectory_publish_period_sec = 30e-3,

rangefinder_sampling_ratio = 1.,

odometry_sampling_ratio = 1.,

fixed_frame_pose_sampling_ratio = 1.,

imu_sampling_ratio = 1.,

landmarks_sampling_ratio = 1.,

===========

map_frame:一般为“map”.用来发布submap的ROS帧ID.

tracking_frame:SLAM算法要跟踪的ROS 帧ID.?

published_frame:用来发布pose的帧ID.?

odom_frame:只要当有里程计信息的时候才会使用。即provide_odom_frame=true.

provide_odom_frame:如果为true，the local, non-loop-closed, continuous pose将会在map_frame 里以odom_frame发布?

publish_frame_projected_to_2d：如果为true，则已经发布的pose将会被完全成２D的pose，没有 roll,pitch或者z-offset？

use_odometry:如果为true，需要提供里程计信息，并话题/odom会订阅nav_msgs/Odometry类型的消息，在SLAM过程中也会使用这个消息进行建图

use_nav_sat:如果为true，会在话题/fix上订阅sensor_msgs/NavSatFix类型的消息，并且在 globalSLAM中会用到

use_landmarks:如果为true，会在话题/landmarks上订阅cartographer_ros_msgs/LandmarkList类型的消息，并且在SLAM过程中会用到

num_laser_scans:SLAM可以输入的/scan话题数目的最大值

num_muti_echo_laser_scans:SLAM可以输入sensor_msgs/MultiEchoLaserScan话题数目的最大值

num_subdivisions_per_laser_scan:将每个接收到的（multi_echo）激光scan分割成的点云数。细分 scam可以在扫描仪移动时取消scanner获取的scan。有一个相应的trajectory builder option可将细分扫描累积到将用于scan_matching的点云中。

num_point_clouds:SLAM可以输入的sensor_msgs/PointCloud2话题数目的最大值

lookup_transform_timeout_sec:使用tf2查找transform的超时时间（秒）。

submap_publish_period_sec:发布submap的时间间隔（秒）

pose_publish_period_sec:发布pose的时间间隔，值为5e-3的时候为200HZ

trajectory_publish_period_sec:发布trajectory markers（trajectory的节点）的时间间隔，值为30e-3 为30ms

rangefinder_samping_ratio:测距仪的固定采样ratio

imu_sampling_ratio:IMU message的固定采样ratio

landmarks_sampling_ratio:landmarks message的固定采样ratio,(单位是啥？)

＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝

4.RVIZ 订阅的topic

1)TF

2)/submap_list

3)/scan_matched_points2

4)/trajectory_node_list

5)/landmark_poses_list

6)/constraint_list

5.App

cartographer_assets_writer

cartographer_dev_trajectory_comparison

cartographer_offline_node

cartographer_rosbag_validate

cartographer_dev_pbstream_trajectories_to_rosbag

cartographer_node

cartographer_pbstream_map_publisher

cartographer_dev_rosbag_publisher

cartographer_occupancy_grid_node

cartographer_pbstream_to_ros_map

6. Note.cpp 源码分析

0.Node构造函数

1) 定义各种topic

submap_list_publisher_ =

node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(

kSubmapListTopic, kLatestOnlyPublisherQueueSize);

trajectory_node_list_publisher_ =

node_handle_.advertise<::visualization_msgs::MarkerArray>(

kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);

landmark_poses_list_publisher_ =

node_handle_.advertise<::visualization_msgs::MarkerArray>(

kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);

constraint_list_publisher_ =

node_handle_.advertise<::visualization_msgs::MarkerArray>(

kConstraintListTopic, kLatestOnlyPublisherQueueSize);

scan_matched_point_cloud_publisher_ =

node_handle_.advertise<sensor_msgs::PointCloud2>(

kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);

2)定义各种服务的发布器。