From 56d9c27df96601b48daad46654507cdc3f2e26d0 Mon Sep 17 00:00:00 2001 From: Christof Groschke Date: Tue, 2 Jun 2020 17:57:18 +0200 Subject: [PATCH] Added a flat_world_odom_node. Cartographer crashed not only when an /imu message arrives out of order, but also when an /odom message is out of order. I will add diagnose files with the next commit so you can verify the issue. I will also add an python script to debug the captured data. A little background: I'm using cartographer with a kinect camera on the turtlebot3 burger + ros melodic. I'm a beginner with ros. Using cartographer in 2D mode worked pretty fine. When i switched to 3D mode, the cartographer crashed a lot with the following error: [FATAL] [1591025245.229967139]: F0601 17:27:25.000000 20863 pose_extrapolator.cc:229] Check failed: time >= imu_tracker->time() (637266220451080319 vs. 637266220451179130) I found out, that the flat_world_imu_node orders the message by timestamp by ignoring messages with a lower timestamp. I wrote a script checking all the topics for this kind of behavior and found out, that odom is not always in the right order. When you take a look at the two files in the next commit called odom_diag.txt and imu_diag.txt you will find an error on Seq 260784 in odom_diag.txt. The error is in the nanoseconds timestamp. The messages are transmitted via 802.11ac. I captured the messages using rostopic echo. I don't know if it helps anyone with the same problem but i hope so. Maybe rename the node before implementing this it is a quick fix for my bachelor thesis. Greetings from germany Happy Pride / Black Lives Matter --- turtlebot3_slam/CMakeLists.txt | 12 +++- .../turtlebot3_slam/flat_world_odom_node.h | 38 +++++++++++++ .../launch/turtlebot3_cartographer.launch | 8 +++ turtlebot3_slam/package.xml | 1 + turtlebot3_slam/src/flat_world_odom_node.cpp | 56 +++++++++++++++++++ 5 files changed, 114 insertions(+), 1 deletion(-) create mode 100644 turtlebot3_slam/include/turtlebot3_slam/flat_world_odom_node.h create mode 100644 turtlebot3_slam/src/flat_world_odom_node.cpp diff --git a/turtlebot3_slam/CMakeLists.txt b/turtlebot3_slam/CMakeLists.txt index 98b487a9..b01721b4 100644 --- a/turtlebot3_slam/CMakeLists.txt +++ b/turtlebot3_slam/CMakeLists.txt @@ -10,6 +10,7 @@ project(turtlebot3_slam) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs + nav_msgs ) ################################################################################ @@ -29,7 +30,8 @@ find_package(catkin REQUIRED COMPONENTS ################################################################################ catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS roscpp sensor_msgs + CATKIN_DEPENDS roscpp sensor_msgs + CATKIN_DEPENDS roscpp nav_msgs ) ################################################################################ @@ -44,6 +46,10 @@ add_executable(flat_world_imu_node src/flat_world_imu_node.cpp) add_dependencies(flat_world_imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(flat_world_imu_node ${catkin_LIBRARIES}) +add_executable(flat_world_odom_node src/flat_world_odom_node.cpp) +add_dependencies(flat_world_odom_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(flat_world_odom_node ${catkin_LIBRARIES}) + ################################################################################ # Install ################################################################################ @@ -55,6 +61,10 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) +install(TARGETS flat_world_odom_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + install(DIRECTORY bag config launch rviz DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/turtlebot3_slam/include/turtlebot3_slam/flat_world_odom_node.h b/turtlebot3_slam/include/turtlebot3_slam/flat_world_odom_node.h new file mode 100644 index 00000000..39818476 --- /dev/null +++ b/turtlebot3_slam/include/turtlebot3_slam/flat_world_odom_node.h @@ -0,0 +1,38 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#ifndef FLAT_WORLD_ODOM_NODE_H_ +#define FLAT_WORLD_ODOM_NODE_H_ + +#include +#include + +class FlatWorldOdomNode +{ + public: + FlatWorldOdomNode(); + ~FlatWorldOdomNode(); + bool init(); + + private: + ros::NodeHandle nh_; + ros::Time last_published_time_; + ros::Publisher publisher_; + ros::Subscriber subscriber_; + void msgCallback(const nav_msgs::Odometry::ConstPtr odom_in); +}; + +#endif // FLAT_WORLD_IMU_NODE_H_ diff --git a/turtlebot3_slam/launch/turtlebot3_cartographer.launch b/turtlebot3_slam/launch/turtlebot3_cartographer.launch index d72f4441..275dfce4 100644 --- a/turtlebot3_slam/launch/turtlebot3_cartographer.launch +++ b/turtlebot3_slam/launch/turtlebot3_cartographer.launch @@ -43,4 +43,12 @@ + + + + + + + + diff --git a/turtlebot3_slam/package.xml b/turtlebot3_slam/package.xml index 98578c01..b54283b9 100644 --- a/turtlebot3_slam/package.xml +++ b/turtlebot3_slam/package.xml @@ -18,6 +18,7 @@ catkin roscpp sensor_msgs + nav_msgs turtlebot3_bringup