Cartographer crashes /odom messages out of order

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}
 )

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 <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+
+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_

turtlebot3_slam/launch/turtlebot3_cartographer.launch

@@ -43,4 +43,12 @@
     <remap from="imu_in" to="/imu" />
     <remap from="imu_out" to="/flat_imu" />
   </node>
+
+  <!-- flat_world_odom_node -->
+  <node pkg="turtlebot3_slam" type="flat_world_odom_node" name="flat_world_odom_node" output="screen">
+    <remap from="odom_in" to="/odom" />
+    <remap from="odom_out" to="/odom_imu" />
+  </node>
+
+
 </launch>

turtlebot3_slam/package.xml

@@ -18,6 +18,7 @@
   <buildtool_depend>catkin</buildtool_depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
   <exec_depend>turtlebot3_bringup</exec_depend>
   <!--  <exec_depend>slam_gmapping</exec_depend>
   <exec_depend>cartographer</exec_depend>

turtlebot3_slam/src/flat_world_odom_node.cpp

@@ -0,0 +1,56 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * 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.
+ */
+
+#include <turtlebot3_slam/flat_world_odom_node.h>
+
+FlatWorldOdomNode::FlatWorldOdomNode()
+{
+  bool init_result = init();
+  ROS_ASSERT(init_result);
+}
+
+FlatWorldOdomNode::~FlatWorldOdomNode()
+{
+}
+
+bool FlatWorldOdomNode::init()
+{
+  publisher_  = nh_.advertise<nav_msgs::Odometry>("odom_out", 10);
+  subscriber_ = nh_.subscribe("odom", 150, &FlatWorldOdomNode::msgCallback, this);
+
+  return true;
+}
+
+void FlatWorldOdomNode::msgCallback(const nav_msgs::Odometry::ConstPtr odom_in)
+{
+  if (last_published_time_.isZero() || odom_in->header.stamp > last_published_time_)
+  {
+    last_published_time_ = odom_in->header.stamp;
+    nav_msgs::Odometry odom_out = *odom_in;
+    publisher_.publish(odom_out);
+  }
+}
+
+int main(int argc, char* argv[])
+{
+  ros::init(argc, argv, "flat_world_odom_node");
+
+  FlatWorldOdomNode flat_world_odom_node;
+
+  ros::spin();
+
+  return 0;
+}