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のキャリブレーションの問題かは不明