cartographer_ros_传感器数据篇

最新推荐文章于 2024-07-13 17:15:55 发布
最新推荐文章于 2024-07-13 17:15:55 发布
阅读量2.4k 收藏 14
点赞数 3
分类专栏： Cartographer 机器人 Graph Optimization
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/caizou123123/article/details/89020701
版权

这篇主要介绍cartographer_ros中传感器数据是如何传递到cartographer中，特别是激光laserscan数据

文章目录

cartographer_ros

node_main.cc

node_main.cc的Run函数作为ros工程的主入口，主要定义了两个变量MapBuilder map_builder_和Node node；如下图所示，　其中map_builder_以参数的形式传递给node

Node node(node_options, std::move(map_builder), &tf_buffer);

在这里插入图片描述

Node

在这里插入图片描述
node_main中MapBuilder map_builder_被作为参数传递给了MapBuilderBridge map_builder_bridge_中用于调用其AddTrajectory. node主要的工作就是进行初始化和启动见图和最后收尾优化的工作

LoadState

void Node::LoadState(const std::string& state_filename, const bool load_frozen_state)
 {
      carto::common::MutexLocker lock(&mutex_);
      map_builder_bridge_.LoadState(state_filename, load_frozen_state);
}

StartTrajectoryWithDefaultTopics

启动核心的AddTrajectory模块

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options)
 {
      carto::common::MutexLocker lock(&mutex_);
      CHECK(ValidateTrajectoryOptions(options));
      AddTrajectory(options, DefaultSensorTopics());
}

AddTrajectory

int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics)
 {
 　　　 const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
     　expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
　　　  const int trajectory_id =　map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  　　　AddExtrapolator(trajectory_id, options);
  　　　AddSensorSamplers(trajectory_id, options);
 　　　 LaunchSubscribers(options, topics, trajectory_id);
 　　　 is_active_trajectory_[trajectory_id] = true;
 　　　 for (const auto& sensor_id : expected_sensor_ids)
 　　　 {
    　　　　　subscribed_topics_.insert(sensor_id.id);
  　　　}
  　　　return trajectory_id;
}

其中　cartographer_ros_msgs::SensroTipics 有
string laser_scan_topic
string multi_echo_laser_scan_topic
string point_cloud2_topic
string imu_topic
string odometry_topic
string nav_sat_fix_topic
string landmark_topic

cartographer::mapping::TrajectoryBuilderInterface::SensorId主要由两个组成:
SensorType type 表示传感器数据类型:激光数据；　IMU数据;　里程计数据; LandMark数据等
string id　　 表示topic

struct SensorId 
{
    enum class SensorType
     {
	      RANGE = 0,
	      IMU,
	      ODOMETRY,
	      FIXED_FRAME_POSE,
	      LANDMARK,
	      LOCAL_SLAM_RESULT
    };

    SensorType type;
    std::string id;
    bool operator==(const SensorId& other) const 
    {
      return std::forward_as_tuple(type, id) == std::forward_as_tuple(other.type, other.id);
    }

    bool operator<(const SensorId& other) const
    {
      return std::forward_as_tuple(type, id) < std::forward_as_tuple(other.type, other.id);
    }
  };

LaunchSubscribers

在这里插入图片描述
在ros端订阅相关的传感器数据，　SubscribeWithHandler

// Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and
// calls 'handler' on the 'node' to handle messages. Returns the subscriber.
template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(void (Node::*handler)(int, const std::string&,const typename MessageType::ConstPtr&),/*　handle function*/
                                      const int trajectory_id, const std::string& topic,
                                      ::ros::NodeHandle* const node_handle, 
                                      Node* const node)
 {
  return node_handle->subscribe<MessageType>(topic, kInfiniteSubscriberQueueSize,
      boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, trajectory_id,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}

订阅话题并启动对应话题的回调处理函数

node_handle->subscribe<MessageType>(topic, kInfiniteSubscriberQueueSize,　boost::function)

boost::function回调函数

 boost::function<void(const typename MessageType::ConstPtr&)>
 (
  [node, handler, trajectory_id,　topic]
 (const typename MessageType::ConstPtr& msg) { (node->*handler)(trajectory_id, topic, msg)})
 )

这里主要void (Node::*handler)(in t, 　const std::string& ,　const typename MessageType::ConstPtr& ) 主要就是对应
　　　　　　　　　　　　　　１　　　　２　　　　　　　　　　　　　　３

// The following functions handle adding sensor data to a trajectory.
  void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg);
  void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,const sensor_msgs::NavSatFix::ConstPtr& msg);
  void HandleLandmarkMessage(int trajectory_id, const std::string& sensor_id,const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
  void HandleImuMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg);
  void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg);
  void HandleMultiEchoLaserScanMessage(int trajectory_id, const std::string& sensor_id,const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
  void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg);

HandleLaserScanMessage

处理LaserScan数据的方法，其他数据类似，　将数据通过map_builder_bridge_从ros端传递到cartographer中

void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) 
{
  carto::common::MutexLocker lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) 
  {
    　return;
  }
   LOG(INFO)<<"Handle Laser Scan Message";
  map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLaserScanMessage(sensor_id, msg);
}

MapBuilderBridge

在这里插入图片描述

SensorBridge

传感器桥接模块，处理消息生成点云数据

void SensorBridge::HandleLaserScanMessage(const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg)
{
	  carto::sensor::PointCloudWithIntensities point_cloud;
	  carto::common::Time time;
	  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
	  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
ToPointCloudWithIntensities
typedef std::vector<Eigen::Vector3f> PointCloud;

// Stores 3D positions of points with their relative measurement time in the
// fourth entry. Time is in seconds, increasing and relative to the moment when
// the last point was acquired. So, the fourth entry for the last point is 0.f.
// If timing is not available, all fourth entries are 0.f. For 2D points, the
// third entry is 0.f (and the fourth entry is time).
typedef std::vector<Eigen::Vector4f> TimedPointCloud;

struct PointCloudWithIntensities 
{
  TimedPointCloud points;
  std::vector<float> intensities;
};
HandleLaserScan
void SensorBridge::HandleLaserScan(const std::string& sensor_id,
                                   const carto::common::Time time,
                                   const std::string& frame_id,
                                   const carto::sensor::PointCloudWithIntensities& points)
{
	  if (points.points.empty())
	  {
	      return;
	  }
     CHECK_LE(points.points.back()[3],  0);
	  // TODO(gaschler): Use per-point time instead of subdivisions.
	  //divid into 10
    for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i)
    {
	    const size_t start_index = points.points.size() * i / num_subdivisions_per_laser_scan_;
	    const size_t end_index   = points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
	    carto::sensor::TimedPointCloud subdivision(
	        points.points.begin() + start_index, points.points.begin() + end_index);
	    if (start_index == end_index)
	    {
	      continue;
	    }
    	const double time_to_subdivision_end = subdivision.back()[3];	
       // `subdivision_time` is the end of the measurement so sensor::Collator will
       // send all other sensor data first.
	    const carto::common::Time subdivision_time = time + carto::common::FromSeconds(time_to_subdivision_end);
	    auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
	    if (it != sensor_to_previous_subdivision_time_.end() &&　it->second >= subdivision_time)
	    {
	      LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
	                   << sensor_id << " because previous subdivision time "
	                   << it->second << " is not before current subdivision time "
	                   << subdivision_time;
	      continue;
	    }
	    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
	    for (Eigen::Vector4f& point : subdivision) 
	    {
	      　point[3] -= time_to_subdivision_end;
	    }
	    CHECK_EQ(subdivision.back()[3], 0);
	    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
	  }
}
HandleRangefinder
void SensorBridge::HandleRangefinder(const std::string& sensor_id,
                                     const carto::common::Time time,
                                     const std::string& frame_id,
                                     const carto::sensor::TimedPointCloud& ranges)
{
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
  if (sensor_to_tracking != nullptr) 
  {

    trajectory_builder_->AddSensorData(sensor_id,
                                       carto::sensor::TimedPointCloudData{time,sensor_to_tracking->translation().cast<float>(),
                                        carto::sensor::TransformTimedPointCloud(ranges, sensor_to_tracking->cast<float>())});
  }
}

trajectory_builder　的定义为::cartographer::mapping::TrajectoryBuilderInterface从
ｍap_builder_->GetTrajectoryBuilder(trajectory_id))中获得，
map_builder.cc中AddTrajectoryBuilder

 trajectory_builders_.push_back(
        common::make_unique<CollatedTrajectoryBuilder>(sensor_collator_.get(),
                                                       trajectory_id,
                                                       expected_sensor_ids,
                                                        CreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder),
                                                                                        trajectory_id,
                                                                                         static_cast<PoseGraph2D*>(pose_graph_.get()),
                                                                                         local_slam_result_callback)));

总结

Node中通过map_builder_bridge_进行操作主要功能AddTrajectory和　HandleLaserScanMessage

小西服红色
关注
  • 3
    点赞
  • 踩
  • 14
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
ROS节点获取TIA_go机器人传感器数据，并上传到指定服务器做深度学习处理
有所为，有所成长
08-19 1466
Brief 正如标题所言，该小项目包括三部分，如下 ROS节点获取机器人数据 socket和服务器数据传输，包括了数据上传和获取 服务器深度学习处理数据 1 ROS节点获取机器人数据 1.1 数据结构 我这两天所负责的内容是读取ROS传感器数据。一开始看到这么个机器人也很懵逼，不知道怎么读数据，经过一些研究后，明白读取数据的过程是一个这样子的流程： 首先，机器人的数据本身就是很多的ROS中...
【cartographer_ros】三： 发布和订阅雷达LaserScan信息
Cloud_H的博客
06-24 3955
上一节介绍和测试了cartographer的官方demo。本节会编写ros系统中，最常用的激光雷达LaserScan传感数据的订阅和发布，方便在cartographer中加入自己的数据进行建图与定位。
Cartographer重入门到精通（二）：运行作者demo及自己的数据集
最新发布
南北粉面馆
07-13 935
这里纯定位的“纯”是相对于建图来说，纯定位的时候地图是已知的，使用之前建立好的地图进行纯粹的定位功能，所以这个demo并不像建图一样占用大量的cpu。这也是carto的亮点，他可以在精度比较差的激光雷达上稳定运行，证明了carto算法的通用性和高效性，并不挑硬件和精度。这个demo是个大杀器，我们知道很多场景是不断变化的，那么我们之前建立的激光地图可能就会失效，那我们频繁的更新激光地图是不是太麻烦，所以我们人为的设置一些路标，就可以大大提高我们定位时的稳定性，实际测试效果也很好。使用生成的地图进行定位。
cartographer代码解读2_cartographer_ros部分
DoctorChen618的专栏
04-19 1598
0 debug 编译 cartographer 版本是在1.0以后，本人fork的是2021.0.25的版本,commit id:0d2bc2ca5b834ddd03504454d0a43b9f67c20455. 编译方式 catkin_make_isolated --install --use-ninja -DFORCE_DEBUG_BUILD=True 1 cartographer_node总结： cartographer启动以后，做了如下的工作： 注册并发布了5个Topic, 并为5个Topic分
cartographer_ros 代码分析1
feidaji的博客
03-10 759
cartographer ROS代码为两大部分：cartographer和cartographer_ros。 cartographer主要负责处理来自雷达、IMU、里程计的数据并基于这些数据进行地图的构建，是cartographer理论的底层实现。 cartographer_ros 则基于ros的通信机制获取传感器的数据并将它们转换成cartographer中定义的格式传递给cartographe...
【cartographer_slam源码阅读】4-6激光雷达数据的转换
憨憨冲鸭
05-11 393
（2）判断雷达原始数据中有强度数据，检查扫描点个数必须与强度数据对齐，即一个扫描点必须对应一个强度信息，满足则push到carto定义的强度数据中，否则push数据0进去；(3)相应的，每个扫描点记录的时间改为points.time-duration（其实就是保证这一帧数据的时间戳加上points.time是每个扫描点的扫描时间）;(2)获取points数据时，最后一个点的points.time数据记录了扫描一帧的用时duration，时间戳为每帧扫描结束后的时间；
从原作者的gitee上下载安装cartographer_ros
05-31
你可以进一步根据自己的需求调整参数，或者使用你自己的传感器数据进行SLAM操作。 Cartographer_ROS提供了丰富的配置选项，包括不同类型的传感器融合、优化参数、地图后处理等。通过深入研究其配置文件和源代码，...
cartographer_ros1.0完整工程
12-23
在Cartographer中，它用于优化传感器数据和地图之间的匹配，以实现更精确的定位和建图。 - **protobuf**：Protocol Buffers是Google的一种数据序列化协议，用于高效地存储和传输结构化数据。在Cartographer中，...
cartographer_turtlebot-master.zip
07-26
Cartographer是由谷歌开发的一种高效、实时的SLAM解决方案，它适用于各种传感器数据，包括激光雷达（LIDAR）和IMU（惯性测量单元）。Cartographer的核心目标是实现在未知环境中移动机器人的自主定位和构建环境地图。...
Cartographer_Visio函数关系总结图.zip
07-19
Cartographer是由Google开发的一款开源SLAM解决方案，主要针对激光雷达（LiDAR）数据，同时也支持其他传感器数据，如IMU和轮速计等。它基于ROS（Robot Operating System，机器人操作系统）平台，为移动机器人提供了...
watersupplyplatform:spring boot + mybatis 实现供水站传感器信息采集和运营维护服务后台
05-18
供水站平台管理系统 简介 spring boot + mybatis 实现供水站传感器信息采集 和运营维护服务后台。 项目需求 1、供水站基本信息 包括供水站基本情况、工程信息、管理情况等。在地图上显示各个站点的 实际位置，将供水站信息叠加在地形图以及影像图上，方便用户进行位置 查询。当某站点当天有预警信息未解除时，该站点显示 进行区分，方便 管理人员查看，同时点击站点可以查看人员巡查、水质、设备运行等实时数据。 2、供水站巡检情况 人员巡查信息模块以日历框形式，初始显示供水站现场管理人员姓名，以及是否按时进行考勤，若有考勤信息，显示已巡查，并显示考勤时间，没有考勤信息，则显示未巡查。巡检若发现异常，巡检人员可通过手机端APP提交巡检记录和现场照片，接收到巡检记录后，以日历框形式予以标识并查询。 3、水质相关信息 水质相关信息模块以日历框形式，初始以不同颜色显示 “水质达标”与“水质不达标
用自己的雷达进行Cartographer建图或仿真
weixin_44492854的博客
11-20 4673
cartographer使用说明
02 传感器的格式转换与类型变换
qq_44178391的博客
09-23 991
学习cartographer的笔记
Cartographer源码阅读---Sensor的数据走向
qq_41663016的博客
03-21 607
Cartographer中传感器数据的传入从Node类的HandleXXXMessage成员函数开始, 传递给SensorBridge类, 然后调用CollatedTrajectoryBuilder把数据给到Collator类, 由Collator进行消费这模式的处理, 然后再返回给CollatedTrajectoryBuilder完成Cartographer的整个前后端.中间的处理用到了很多父子类的互相调用, 一层套一层, 十分复杂, 要认真看才能懂里面的数据流.
Cartographer源码阅读3D-前端数据处理-IMU&3D点云
yeluohanchan的博客
10-09 1017
Cartographer源码阅读2D&3D-3D前端数据处理-IMU&3D点云 3D需要传入的数据包括：IMU（必须），点云数据(sensor_msgs::MultiEchoLaserScan或sensor_msgs::PointCloud2)，轮速计数据(可选，nav_msgs::Odometry，和2D相同，不再分析) IMU数据的处理 通过接收sensor_msgs::Imu数据，转换为sensor::ImuData数据： struct ImuData { // 时间 comm
25-总结-【cartographer源码分析】系列的第三部分【sensor源码分析】
slamcode的博客
07-27 3638
【cartographer源码分析】系列的第三部分【sensor源码分析】。
Cartographer源码阅读2D&3D-前端根据时间戳滤波-RangeDataCollator
yeluohanchan的博客
09-18 788
Cartographer源码阅读-2D前端-根据时间戳滤波-RangeDataCollator 基本原理： 根据当前激光帧的时间戳和上一帧激光帧的时间戳，找到当前激光帧和上一帧激光帧时间重叠的激光点，滤除时间重复的点。 class RangeDataCollator { public: explicit RangeDataCollator( const std::vector<std::string>& expected_range_sensor_ids)
anzhuang cartographer_ros
12-20
安装cartographer_ros的步骤如下： 1. 首先，确保你已经安装了ROS... 注意：在运行cartographer_ros之前，你需要根据你的传感器类型和配置进行相应的参数设置。具体的配置方法可以参考cartographer_ros的官方文档。
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值