ROS33:cartgrapher(3D)を動かす。 

Googleのcartographerを３D動かすところ（エラーが発生しなくなる）までの備忘録です。まだ使い物になっていません。IMUが。。。
@yonekenさんの、https://github.com/ros-japan-users/cartographer_example を参考にしています。

#使用機器
Hokuyo UST-10LX 2台

IMU razor 9dof 
https://www.switch-science.com/catalog/679/
https://github.com/KristofRobot/razor_imu_9dof

#事前知識
##ROSのIMUとの向き
http://wiki.ros.org/razor_imu_9dof indigoのバージョンに
    X axis pointing forward (towards the short edge with the connector holes)
    Y axis pointing to the left
    Z axis pointing up 

って書かれているから、セッテイング
合っていますか？

#はまりどころ
laserscanトピックをremapしないと以下のエラーが発生します。

```
[ INFO] [1495933910.028762966]: I0528 10:11:50.000000  3703 kalman_local_trajectory_builder.cc:77] PoseTracker not yet initialized.
F0528 10:11:50.073985  3703 ordered_multi_queue.cc:112] Check failed: last_dispatched_time_ <= next_data->time (636315307100542090 vs. 636315307100479828) Non-sorted data added to queue: '(0, scan)'
[FATAL] [1495933910.074488052]: F0528 10:11:50.000000  3703 ordered_multi_queue.cc:112] Check failed: last_dispatched_time_ <= next_data->time (636315307100542090 vs. 636315307100479828) Non-sorted data added to queue: '(0, scan)'
*** Check failure stack trace: ***
    @     0x7faef16bd5cd  google::LogMessage::Fail()
    @     0x7faef16bf433  google::LogMessage::SendToLog()
    @     0x7faef16bd15b  google::LogMessage::Flush()
    @     0x7faef16bfe1e  google::LogMessageFatal::~LogMessageFatal()
    @           0x5c18bf  cartographer::sensor::OrderedMultiQueue::Dispatch()
    @           0x5c2b0f  cartographer::sensor::OrderedMultiQueue::Add()
    @           0x5bfd64  cartographer::sensor::Collator::AddSensorData()
    @           0x5cad78  cartographer::mapping::CollatedTrajectoryBuilder::AddSensorData()
    @           0x5160f6  cartographer_ros::SensorBridge::HandleImuMessage()
    @           0x4eb85b  _ZN5boost6detail8function26void_function_obj_invoker1IZN16cartographer_ros12_GLOBAL__N_13RunEvEUlRKNS_10shared_ptrIKN11sensor_msgs4Imu_ISaIvEEEEEE2_vSD_E6invokeERNS1_15function_bufferESD_
    @           0x4f2a6e  boost::detail::function::void_function_obj_invoker1<>::invoke()
    @           0x4fb0c9  ros::SubscriptionCallbackHelperT<>::call()
    @     0x7faeee8255dd  ros::SubscriptionQueue::call()
    @     0x7faeee7cfcf0  ros::CallbackQueue::callOneCB()
    @     0x7faeee7d10f3  ros::CallbackQueue::callAvailable()
    @     0x7faeee8296a1  ros::SingleThreadedSpinner::spin()
    @     0x7faeee80e73b  ros::spin()
    @           0x4ecbde  cartographer_ros::(anonymous namespace)::Run()
    @           0x4e93c4  main
    @     0x7faeed228830  __libc_start_main
    @           0x4ead99  _start
    @              (nil)  (unknown)
[cartographer_node-7] process has died [pid 3703, exit code -6, cmd /home/tauchi/catkin_ws/install_isolated/lib/cartographer_ros/cartographer_node -configuration_directory /home/tauchi/catkin_ws/install_isolated/share/cartographer_example/configuration_files -configuration_basename urg04x_3d.lua __name:=cartographer_node __log:=/home/tauchi/.ros/log/992700b2-4342-11e7-9159-d481d76989a7/cartographer_node-7.log].
```

#動いたもの
```:urg_3d.launch
<launch>
<!-- param name="/use_sim_time" value="true" / -->
  <node name="horizontal_laser" pkg="urg_node"
    type="urg_node" >
    <param name="ip_address" value="192.168.1.31" />
    <param name="frame_id" value="horizontal_laser_link" />
    <remap from="scan" to="horizontal_laser_3d" />
  </node>

  <node name="vertical_laser" pkg="urg_node"
    type="urg_node" >
    <param name="ip_address" value="192.168.1.32" />
    <param name="frame_id" value="vertical_laser_link" />
    <remap from="scan" to="vertical_laser_3d" />
  </node>

  <include file="$(find razor_imu_9dof)/launch/razor-pub.launch" />

  <node pkg="tf" type="static_transform_publisher" name="imu_link_connect" args="0 0 0 0 0 0 /imu_link /base_imu_link 100"/>

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf"/>

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_example)/configuration_files
          -configuration_basename urg04x_3d.lua"
      output="screen">
    <remap from="scan" to="horizontal_laser_3d" />
    <remap from="scan" to="vertical_laser_3d" />
  </node>

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
</launch>
```

```:urg04x_3d.lua
-- 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 "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  use_odometry = false,
  use_laser_scan = true,
  use_multi_echo_laser_scan = false,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
}

TRAJECTORY_BUILDER_3D.scans_per_accumulation = 16

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 3
MAP_BUILDER.sparse_pose_graph.optimization_problem.huber_scale = 5e2
MAP_BUILDER.sparse_pose_graph.optimize_every_n_scans = 320
MAP_BUILDER.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
MAP_BUILDER.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10
-- Reuse the coarser 3D voxel filter to speed up the computation of loop closure
-- constraints.
MAP_BUILDER.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter
MAP_BUILDER.sparse_pose_graph.constraint_builder.min_score = 0.62


return options
```

#現状
どんどん上昇していきます。IMUのキャリブレーションの問題かは不明