submap_slices and last_frame_id is empty

AndreasAZiegler commented 5 years ago

I set up turtlebot3 with ROS2 dashing. After I start gazebo (with use_sim_time True), I start cartographer with: ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True

Rviz2 tells me that there is no map frame and I get a lot of the following warnings from the occupancy_grid_node: [occupancy_grid_node-2] [WARN] [occupancy_grid_node]: submap_slices and last_frame_id is empty

dingyi9982 commented 5 years ago

I met the same issue, and I tried to debug the cartographer package both on Crystal which ran very well and on Dashing which printed the warning and no map in RViz2。 I found that, on Crystal, "imu", "odom" and "scan" topics published by turtlebot3_gazebo package could be handled in node.cc by function HandleImuMessage, HandleOdometryMessage and HandleLaserScanMessage, but on Dashing, only "odom" topic could be handled, by HandleOdometryMessage. Sorry for that, I am Chinese, so I recorded the debugging of cartographer with Chinese, as follows:

在Ubuntu18.04 + Dashing中运行TurtleBot3的仿真时遇到了如下问题：在运行：ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True 时一直打印如下warning：[WARN] [occupancy_grid_node]: submap_slices and last_frame_id is empty

然后rviz2中的地图无法显示，通过ros2 topic echo /map看不到任何消息，说明turtlebot3_cartographer可能没有运行成功。在Ubuntu18.04 + Dashing（存在上述异常情况）和Ubuntu18.04+Crystal（不存在异常情况）都进行调试，记录如下：

用colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug -DFORCE_DEBUG_BUILD=True重新编译cartographer。
由于打印的warning是occupancy_grid_node节点打印的，所以用vscode调试occupancy_grid_node对应源代码文件occupancy_grid_node_main.cc，上述warning是其中的DrawAndPublish函数打印的，调试发现submapslices.empty()和last_frameid.empty()均为true，进一步调试分析发现，当occupancy_grid_node接收到/submap_list消息时，会调用handleSubmapList函数，然后该函数会设置submap_slices_和last_frame_id_的值。
通过ros2 node info cartographer_node发现/submap_list是cartographer_node节点发布的，再继续调试cartographer_node对应的源代码文件node.cc，经调试发现GetSubmapList函数的如下代码：
```
for (const auto& submap_id_pose :
      map_builder_->pose_graph()->GetAllSubmapPoses()) {
   cartographer_ros_msgs::msg::SubmapEntry submap_entry;
   submap_entry.trajectory_id = submap_id_pose.id.trajectory_id;
   submap_entry.submap_index = submap_id_pose.id.submap_index;
   submap_entry.submap_version = submap_id_pose.data.version;
   submap_entry.pose = ToGeometryMsgPose(submap_id_pose.data.pose);
   submap_list.submap.push_back(submap_entry);
 }
```
在Dashing中没有进去for循环，而Crystal中则进去了。进一步分析，将断点打在SubscribeWithHandler函数的return语句处，并查看topic的值发现，Crystal中，topic的值在"imu", "odom"和"scan"之间变动，而Dashing中，topic的值保持"odom"不变，即Dashing中，cartographer_node没有处理"imu"和"scan"两个topic，所以也就没有调用HandleImuMessage和HandleLaserScanMessage，而Dashing中通过ros2 topic echo /imu和ros2 topic echo /scan能看到publish的topic内容，说明发布没问题，同时，LaunchSubscribers函数中订阅/imu消息的代码也似乎没问题，具体原因有待进一步调试。

ZakariaChekakta commented 5 years ago

@AndreasAZiegler , @dingyi9982 is this issue solved?, because I am facing it now

ros2 / cartographer_ros

submap_slices and last_frame_id is empty #27