FAST-LIVO2

0. 우선 isaac sim환경을 띄운다.

 

 

---

 

1. 우선 필요한거 설치

sudo apt install \
  ros-humble-navigation2 \
  ros-humble-nav2-bringup \
  ros-humble-slam-toolbox \
  ros-humble-pointcloud-to-laserscan \
  ros-humble-topic-tools \
  ros-humble-tf2-tools \
  ros-humble-rviz2 \
  ros-humble-nav2-map-server

 

 

---

 

2. ros2 topic echo /carter1/chassis/odom --once 을 입력해서, 토픽의 frame_id와 child_frame_id를 확인하자.

• 토픽 echo를 보면, 모바일 로봇의 frame_id와 child_frame_id를 확인할수 있다.
• 여기에서는 frame_id는 odom이다.
  • 이는 기준 프레임을 뜻한다.
• child_frame_id는 base_link이다.
  • 이는 로봇 본체의 프레임이다.

 

 

---

 

 

3. ros2에서 PointCloud2형식의 LiDAR PointCloud topic을 Livox customMsg형식 topic으로 변환하는 bridge노드 실행.

ros2 run pointcloud2_to_livox_bridge pc2_to_livox   --ros-args   -p input:=/carter1/front_3d_lidar/lidar_points   -p output:=/carter1/front_3d_lidar/livox_custom   -p max_points:=5000   -p scan_period_ns:=27777777

• 위의 명령어는 pointcloud2_to_livox_bridge 패키지 안에 있는 pc2_to_livox 실행파일을 실행한다.
• input:= 
  • 뒤에 나오는 토픽이 subscribe할 토픽이다.
• output:=
  • 출력토픽이다. 즉, Livox 형식 메시지로 변환된 토픽으로 publish하는것.

subscribers를 보면 input:=에 입력한 토픽이 보인다.

• -p max_points:=30000
  • 한 스캔/frame에서 최대 30,000개 포인트까지만 변환하거나 담겠다는 뜻이다.
• -p scan_period_ns:=27,777,777
  • 스캔 주기를 나노초 단위로 지정한것.
  • 0.027 777 777초 = 27.78ms이다.

 

---

 

4. FAST_LIVO mapping node를 launch파일로 실행하는 명령어

ros2 launch fast_livo mapping_aviz.launch.py use_sim_time:=True use_rviz:=False

• 이를 실행하면, FAST-LIVO의 mapping,odometry 관련 노드들을 한번에 띄운다. 

 

---

 

5. 이제 fast-livo2의 pose로 camera_init --> odom 형태의 TF를 만들어야 한다.

• 또한 Nav2는 camera --> odom --> base_link형태의 TF를 볼수 있어야 한다.
• 위의 2번에서 carter로봇은 이미 odom --> base_link 형태의 TF를 가지고 있는거를 확인함.
• FAST-LIVO2는 camera_init --> base_link 를 내보낸다.
  • ==> 그래서 위의 2개를 이용해서 camera_init --> odom을 만들어야 한다. 
  • 수식으로 쓴다면...
    • camera_init --> base_link  = camera_init --> odom * odom --> base_link 
  • 여기서 Nav2는 보통 map --> odom --> base_link구조의 TF이다. 하지만 여기서는 FAST-LIVO2의 전역프레임인 camera_init을 쓰는 구조로 할 것 이다.

---

 

6. 위에서 말한 내용에 따라 bridge 패키지를 만든다.

cd ~/fast_ws/src
ros2 pkg create fast_livo_nav2_bridge   --build-type ament_python   --dependencies rclpy nav_msgs geometry_msgs tf2_ros

vim ~/fast_ws/src/fast_livo_nav2_bridge/fast_livo_nav2_bridge/fast_livo_map_odom_tf.py

fast_livo_map_odom_tf.py

0.00MB

vim ~/fast_ws/src/fast_livo_nav2_bridge/setup.py

setup.py

0.00MB

 

cd ~/fast_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
colcon build --symlink-install --packages-select fast_livo_nav2_bridge
source install/setup.bash

 

---

7. 위에서 만든 패키지를 실행한다.

ros2 run fast_livo_nav2_bridge fast_livo_map_odom_tf   --ros-args   -p fast_odom_topic:=/aft_mapped_to_init   -p wheel_odom_topic:=/carter1/chassis/odom   -p map_frame:=camera_init   -p odom_frame:=odom

• 이걸로 FAST-LIVO2 pose와 carter wheel odom을 받아서, camera_init --> odom TF를 만들어주는 node실행 명령어이다.
• 노드 이름은 fast_livo_map_odom_tf로 실행된다.
• fast_odom_topic:=
  • 이는 FAST-LIVO2가 내보내는 odometry 토픽이름이다. 그래서 해당 노드는 이를 구독한다.
• wheel_odom_topic:=
  • 이는 carter가 원래 가지고 있는 wheel/chassis odometry 토픽이다.
• map_frame:=
  • 새로 만들 TF의 부모 프레임 이름이다.
• odom_frame:=
  • 새로 만들 TF의 자식 프레임 이름이다.

 

---

 

8. base_link를 기준으로 front_3d_lidar가 어디에 장착되어 있는지 알려주는 고정 TF를 publish한다.

ros2 run tf2_ros static_transform_publisher \
  -0.23169997334480286 0.0 0.5260000228881836 \
  0 0 0 \
  base_link front_3d_lidar

 

 

---

 

9.  nav2 obstacle layer는 보통 laserScan을 사용한다. 그래서 3D LiDAR를 /scan( 2D laserscan )으로 변환한다.

• ros2 run pointcloud_to_laserscan pointcloud_to_laserscan_node   --ros-args   -r cloud_in:=/carter1/front_3d_lidar/lidar_points   -r scan:=/scan   -p min_height:=-1.0   -p max_height:=1.0   -p angle_min:=-3.14159   -p angle_max:=3.14159   -p angle_increment:=0.0087   -p range_min:=0.20   -p range_max:=20.0   -p use_inf:=true   -p queue_size:=50
  
  

 

---

 

10. nav2가 만든 속도 명령을 carter가 실제로 받는 topic으로 전달해준다.

ros2 run topic_tools relay /cmd_vel /carter1/cmd_vel

• 보통 nav2는 로봇을 움직일때 /cmd_vel로 명령을 낸다.
• 하지만 carter는 /carter1/cmd_vel을 듣고 있다.
  그래서 그냥 두면 로봇이 안움직이니, 이 명령어로 움직이게 한다.(Nav2만 켜면 Nav2는 열심히 /cmd_vel을 내보내지만, Carter는 그걸 안 듣는다. 그래서 로봇이 안움직임.)

 

---

11. nav2 navigation을 실행한다. 그리고 rviz를 띄운다.

ros2 launch nav2_bringup navigation_launch.py   use_sim_time:=True   params_file:=/home/jihong/nav2_fast_livo/config/nav2_params.yaml
ros2 launch nav2_bringup rviz_launch.py use_sim_time:=True

nav2_params.yaml

0.01MB