[작업 일지] cartographer - mapping and localization

이전 내용

 - rrt를 작업하다가 localization도 cartographer로 할 수 있다는 사실을 알게 되었다. Bresenham 알고리즘으로 localization node를 만들어 쓰고 있었는데, (그것도 문제를 안고 있었지만) 성능이 좀 더 개선될 거라 기대하며 cartographer로 pure localization을 구현하게 됐다. 

작업 내용 

1. 기존 localization에 쓰는 설정 파일(lua)과 실행 파일(launch)를 살펴봤다. 설정 파일에는 별 내용이 없이 pure_localization_trimmer와 optimize_every_n_nodes만 있었다. 

-- filename: cartographer_ros/configuration_files/backpack_2d_localization.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 "backpack_2d.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options

 

 실행파일에서는 mapping과 달리 cartographer node에 -load_state_filename 인수로 설정하는 것이 있었다. pbstream file은 localization을 마무리하고 cartographer service로 받아야 한다.

 그리고 occupancy_grid_node를 mapping과 같은 방식으로 불러왔다. 

<!--filename: cartographer_ros/launch/demo_backpack_2d_localization.launch-->
<launch>
  <param name="/use_sim_time" value="true" />

  <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_ros)/configuration_files
          -configuration_basename backpack_2d_localization.lua
          -load_state_filename $(arg load_state_filename)"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />
</launch>

 

2. 깃허브에서 확인하는 코드와 apt를 이용해서 설치한 cartographer의 버전이 다를 수 있기 때문에 짚고 넘어가자면

cartographer/package.xml에서는 2.0.0버전이고 설치한 버전은 1.0.9001이었다. 문제가 안 생기길 바라면서 ^^

<package format="3">
  <name>cartographer</name>
  <version>2.0.0</version>
  <description>
    Cartographer is a system that provides real-time simultaneous localization
    and mapping (SLAM) in 2D and 3D across multiple platforms and sensor
    configurations.
  </description>
...

dawgs_nx@dawgs:~/f1tenth_dawgs$ cat /opt/ros/foxy/share/cartographer/package.xml 
  <version>1.0.9001</version>
  ...

 

3. 실행파일과 설정파일을 따라 만들었고 바로 실행했다. pure_localization_trimmer에서 문제가 생겼다. 분명 위에 설명대로 lua파일을 작성하고 build했지만 에러는 계속 그대로였다. copilot한테 물어봐도 제대로 설정하라는 얘기였다. 혹시나 해서 지우고 해봤더니 다른 에러로 옮겨갔다. 

dawgs_nx@dawgs:~/f1tenth_dawgs$ ros2 launch agent_dawgs localization_cartographer_ekf.launch.py
[INFO] [cartographer_node-12]: process started with pid [5676]
[INFO] [occupancy_grid_node-13]: process started with pid [5678]
[occupancy_grid_node-13] 1753866617.564580 [0] occupancy_: using network interface wlan0 (udp/192.168.0.8) selected arbitrarily from: wlan0, docker0
[cartographer_node-12] F0730 18:10:17.747864  5676 lua_parameter_dictionary.cc:410] Check failed: 1 == reference_counts_.count(key) (1 vs. 0) Key 'pure_localization_trimmer' was used the wrong number of times.
[cartographer_node-12] [FATAL] [1753866617.753515193] [cartographer_ros]: F0730 18:10:17.000000  5676 lua_parameter_dictionary.cc:410] Check failed: 1 == reference_counts_.count(key) (1 vs. 0) Key 'pure_localization_trimmer' was used the wrong number of times.
[ERROR] [cartographer_node-12]: process has died [pid 5676, exit code -6, \
cmd '/opt/ros/foxy/lib/cartographer_ros/cartographer_node -configuration_directory /home/dawgs_nx/f1tenth_dawgs/install/agent_dawgs/share/agent_dawgs/config \
-configuration_basename cartographer_localization_2d.lua -load_state_filename /home/dawgs_nx/f1tenth_dawgs/src/peripheral/maps/ekf_false_20250724_222910.pbstream -minloglevel 2 \
--ros-args -r __node:=cartographer_node --params-file /tmp/launch_params_5nuzc_5e -r odom:=odometry/filtered'].

 아래에서 pure_localization_trimmer를 주석처리한 다음 실행을 했을 때, publish_tracked_pose로 에러가 바뀌었다. 뭔가 이상한 것을 알아차렸지만 감이 없어서 그냥 다시 trimmer를 넣었다. 

[cartographer_node-12] F0730 22:25:14.555362 17688 lua_parameter_dictionary.cc:410] Check failed: 1 == reference_counts_.count(key) (1 vs. 0) Key 'publish_tracked_pose' was used the wrong number of times.
[cartographer_node-12] [FATAL] [1753881914.559706795] [cartographer_ros]: F0730 22:25:14.000000 17688 lua_parameter_dictionary.cc:410] Check failed: 1 == reference_counts_.count(key) (1 vs. 0) Key 'publish_tracked_pose' was used the wrong number of times.

 

4. 에러코드에서 lua_parameter_dictonary.cc:410가 있었으니 근처를 확인해보려고 했다. 다행히도 코드가 그대로 살아있는 듯했다. 

void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() {
  for (const auto& key : GetKeys()) {
    CHECK_EQ(1, reference_counts_.count(key))
        << "Key '" << key << "' was used the wrong number of times.";
    CHECK_EQ(1, reference_counts_.at(key))
        << "Key '" << key << "' was used the wrong number of times.";
  }
  reference_counts_.clear();
}

 해석은 코팔럿한테 맡겨보니. reference_counts_.count는 맵에 키가 있는지 확인하는 것이고 reference_counts_.at은 사용 횟수를 확인하는 것이었다.

 위에서 발견했던 pure_localization_trimmer를 주석처리했을 때와 생각하면 실제로 파라미터 맵에 pure_localization_trimmer가 없다는 얘기가 된다. 그러니 주석처리해야할 부분이 되는 것이다. (앞서 다른 에러로 바뀌었다는 publish_tracked_pose와 publish_to_tf도 주석처리했다.)

 

5. localization을 이제 하고보니 속도가 빠르면 odometry가 너무 벗어나게 되는 문제가 있었다. 설정파일을 최적화 하면서 odom이 너무 안튀게 만들어야 하겠다. 

잘했던부분

- 

아쉬웠던 부분

- 실행파일을 너무 너덜너덜해졌다. 

 

참고자료

- cartographer_ros localization launch: cartographer_ros/cartographer_ros/launch/demo_backpack_2d_localization.launch at master · cartographer-project/cartographer_ros

- cartographer package.xml: cartographer/package.xml at master · cartographer-project/cartographer

- lua_parameter_dictonary.cc: https://github.com/cartographer-project/cartographer/blob/master/cartographer/common/lua_parameter_dictionary.cc#L408

- cartographer lua file tuning guide: Tuning methodology — Cartographer ROS documentation