Adds flat world IMU publisher for TurtleBot 2. (#4)

cartographer_turtlebot2/CMakeLists.txt

@@ -16,9 +16,36 @@ cmake_minimum_required(VERSION 2.8.3)
 
 project(cartographer_turtlebot2)
 
-find_package(catkin)
+set(PACKAGE_DEPENDENCIES
+  roscpp
+  sensor_msgs
+)
+
+set(CMAKE_CXX_FLAGS "-pthread -std=c++11 -Wreorder")
+
+if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "")
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
+if(CMAKE_BUILD_TYPE STREQUAL "Release")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -DNDEBUG")
+elseif(CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -g -DNDEBUG")
+elseif(CMAKE_BUILD_TYPE STREQUAL "Debug")
+  message(FATAL_ERROR "Cartographer is too slow to be useful in debug mode.")
+else()
+  message(FATAL_ERROR "Unknown CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}")
+endif()
+
+message(STATUS "Build type: ${CMAKE_BUILD_TYPE}")
+
+find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
+
+catkin_package(CATKIN_DEPENDS message_runtime ${PACKAGE_DEPENDENCIES})
 
-catkin_package()
+add_executable(flat_world_imu_node src/flat_world_imu_node_main.cc)
+target_link_libraries(flat_world_imu_node ${catkin_LIBRARIES})
+add_dependencies(flat_world_imu_node ${catkin_EXPORTED_TARGETS})
 
 install(DIRECTORY launch/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/

cartographer_turtlebot2/configuration_files/demo_turtlebot2.rviz

@@ -224,8 +224,8 @@ Visualization Manager:
       Enabled: true
       Map frame: map
       Name: Submaps
-      Submap query service: /cartographer/submap_query
-      Topic: /cartographer/submap_list
+      Submap query service: /cartographer_node/submap_query
+      Topic: /cartographer_node/submap_list
       Tracking frame: base_link
       Unreliable: false
       Value: true

cartographer_turtlebot2/configuration_files/turtlebot2.lua

@@ -17,7 +17,7 @@ include "map_builder.lua"
 options = {
   map_builder = MAP_BUILDER,
   map_frame = "map",
-  tracking_frame = "base_link",
+  tracking_frame = "gyro_link",
   odom_frame = "odom",
   provide_odom_frame = false,
   use_odometry_data = true,
@@ -37,7 +37,8 @@ options = {
 }
 
 options.map_builder.use_trajectory_builder_2d = true
-options.map_builder.trajectory_builder_2d.use_imu_data = false
+options.map_builder.trajectory_builder_2d.use_imu_data = true
 options.map_builder.trajectory_builder_2d.use_online_correlative_scan_matching = true
+options.map_builder.trajectory_builder_2d.motion_filter.max_angle_radians = math.rad(0.25)
 
 return options

cartographer_turtlebot2/launch/demo_turtlebot2.launch

@@ -17,12 +17,17 @@
 <launch>
   <param name="/use_sim_time" value="true" />
 
-  <node name="cartographer" pkg="cartographer_ros"
+  <node name="cartographer_node" pkg="cartographer_ros"
       type="cartographer_node" args="
           -configuration_directory
               $(find cartographer_turtlebot2)/configuration_files
           -configuration_basename turtlebot2.lua"
       output="screen" />
+  <node name="flat_world_imu_node" pkg="cartographer_turtlebot2"
+      type="flat_world_imu_node" output="screen">
+    <remap from="imu_in" to="/mobile_base/sensors/imu_data_raw" />
+    <remap from="imu_out" to="/imu" />
+  </node>
   <node name="rviz" pkg="rviz" type="rviz" required="true"
       args="-d $(find cartographer_turtlebot2
           )/configuration_files/demo_turtlebot2.rviz" />

cartographer_turtlebot2/launch/turtlebot2.launch

@@ -18,10 +18,16 @@
   <param name="robot_description"
     textfile="$(find cartographer_turtlebot2)/urdf/turtlebot2.urdf" />
 
-  <node name="cartographer" pkg="cartographer_ros"
+  <node name="cartographer_node" pkg="cartographer_ros"
       type="cartographer_node" args="
           -configuration_directory
               $(find cartographer_turtlebot2)/configuration_files
           -configuration_basename turtlebot2.lua"
       output="screen" />
+  </node>
+  <node name="flat_world_imu_node" pkg="cartographer_turtlebot2"
+      type="flat_world_imu_node" output="screen">
+    <remap from="imu_in" to="/mobile_base/sensors/imu_data_raw" />
+    <remap from="imu_out" to="/imu" />
+  </node>
 </launch>

cartographer_turtlebot2/package.xml

@@ -30,11 +30,16 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>g++-static</build_depend>
+
   <depend>cartographer_ros</depend>
   <depend>depthimage_to_laserscan</depend>
   <depend>kobuki</depend>
   <depend>kobuki_core</depend>
+  <depend>message_runtime</depend>
   <depend>openni2_launch</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
   <depend>turtlebot_bringup</depend>
   <depend>urg_node</depend>
 </package>

cartographer_turtlebot2/src/flat_world_imu_node_main.cc

@@ -0,0 +1,58 @@
+/*
+ * 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 "ros/ros.h"
+#include "sensor_msgs/Imu.h"
+
+namespace {
+
+constexpr double kFakeGravity = 9.8;
+constexpr int kSubscriberQueueSize = 150;
+constexpr char kImuInTopic[] = "imu_in";
+constexpr char kImuOutTopic[] = "imu_out";
+
+}  // namespace
+
+int main(int argc, char** argv) {
+  ::ros::init(argc, argv, "flat_world_imu_node");
+
+  ros::Time last_time;
+  ::ros::NodeHandle node_handle;
+  ::ros::Publisher publisher =
+      node_handle.advertise<sensor_msgs::Imu>(kImuOutTopic, 10);
+  ::ros::Subscriber subscriber = node_handle.subscribe(
+      kImuInTopic, kSubscriberQueueSize,
+      boost::function<void(const sensor_msgs::Imu::ConstPtr& imu_in)>(
+          [&](const sensor_msgs::Imu::ConstPtr& imu_in) {
+            // The 'imu_data_raw' topic of the Kobuki base will at times publish
+            // IMU messages out of order. These out of order messages must be
+            // dropped.
+            if (last_time.isZero() || imu_in->header.stamp > last_time) {
+              sensor_msgs::Imu imu_out = *imu_in;
+              // TODO(damonkohler): This relies on the z-axis alignment of the
+              // IMU with the Kobuki base.
+              imu_out.linear_acceleration.x = 0.;
+              imu_out.linear_acceleration.y = 0.;
+              imu_out.linear_acceleration.z = kFakeGravity;
+              publisher.publish(imu_out);
+            }
+            last_time = imu_in->header.stamp;
+          }));
+
+  ::ros::start();
+  ::ros::spin();
+  ::ros::shutdown();
+}