RTAB_MAP시에 imu센서 정보도 더해서 동작

 

https://jihong.tistory.com/666

rtabmap으로 3d 지도 작성

 

 

 

 

1. 패키지 생성하기

mkdir -p ~/ros2_ws_imu/src
cd ~/ros2_ws_imu

cd ~/ros2_ws_imu/src
ros2 pkg create --build-type ament_python imu_tools

cd ~/ros2_ws_imu/src/imu_tools/imu_tools
touch combine_imu.py

 

 

 

2. combine_imu.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from collections import deque
from rclpy.qos import QoSProfile, ReliabilityPolicy

class ImuCombiner(Node):
    def __init__(self):
        super().__init__('imu_combiner')
        self.accel_queue = deque(maxlen=10)
        self.gyro_queue = deque(maxlen=10)
        best_effort_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            depth=10
        )
        self.accel_sub = self.create_subscription(
            Imu, '/camera/camera/accel/sample', self.accel_callback, best_effort_qos)
        self.gyro_sub = self.create_subscription(
            Imu, '/camera/camera/gyro/sample', self.gyro_callback, best_effort_qos)
        self.imu_pub = self.create_publisher(Imu, '/imu/data', 10)

    def accel_callback(self, msg):
        self.accel_queue.append(msg)
        self.try_publish()

    def gyro_callback(self, msg):
        self.gyro_queue.append(msg)
        self.try_publish()

    def try_publish(self):
        if not self.accel_queue or not self.gyro_queue:
            return

        # Find accel/gyro msg pair with closest timestamp
        accel_msg = self.accel_queue[-1]
        min_dt = None
        best_gyro = None
        for gyro_msg in self.gyro_queue:
            dt = abs(self.to_stamp(accel_msg) - self.to_stamp(gyro_msg))
            if min_dt is None or dt < min_dt:
                min_dt = dt
                best_gyro = gyro_msg

        if best_gyro is None or min_dt > 0.01:  # 0.01초(10ms) 이내만 사용
            return

        imu_msg = Imu()
        imu_msg.header.stamp = accel_msg.header.stamp
        imu_msg.header.frame_id = accel_msg.header.frame_id
        imu_msg.linear_acceleration = accel_msg.linear_acceleration
        imu_msg.linear_acceleration_covariance = accel_msg.linear_acceleration_covariance
        imu_msg.angular_velocity = best_gyro.angular_velocity
        imu_msg.angular_velocity_covariance = best_gyro.angular_velocity_covariance
        imu_msg.orientation = accel_msg.orientation
        imu_msg.orientation_covariance = accel_msg.orientation_covariance
        self.imu_pub.publish(imu_msg)

    def to_stamp(self, msg):
        return msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

def main(args=None):
    rclpy.init(args=args)
    node = ImuCombiner()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

 

or

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion
from collections import deque
from rclpy.qos import QoSProfile, ReliabilityPolicy

class ImuCombiner(Node):
    def __init__(self):
        super().__init__('imu_combiner')
        self.accel_queue = deque(maxlen=10)
        self.gyro_queue = deque(maxlen=10)
        best_effort_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            depth=10
        )
        self.accel_sub = self.create_subscription(
            Imu, '/camera/camera/accel/sample', self.accel_callback, best_effort_qos)
        self.gyro_sub = self.create_subscription(
            Imu, '/camera/camera/gyro/sample', self.gyro_callback, best_effort_qos)
        self.imu_pub = self.create_publisher(Imu, '/imu/data', 10)

    def accel_callback(self, msg):
        self.accel_queue.append(msg)
        self.try_publish()

    def gyro_callback(self, msg):
        self.gyro_queue.append(msg)
        self.try_publish()

    def try_publish(self):
        if not self.accel_queue or not self.gyro_queue:
            return

        accel_msg = self.accel_queue[-1]
        min_dt = None
        best_gyro = None
        for gyro_msg in self.gyro_queue:
            dt = abs(self.to_stamp(accel_msg) - self.to_stamp(gyro_msg))
            if min_dt is None or dt < min_dt:
                min_dt = dt
                best_gyro = gyro_msg

        if best_gyro is None or min_dt > 0.01:
            return

        imu_msg = Imu()
        imu_msg.header.stamp = accel_msg.header.stamp
        imu_msg.header.frame_id = accel_msg.header.frame_id
        imu_msg.linear_acceleration = accel_msg.linear_acceleration
        imu_msg.linear_acceleration_covariance = accel_msg.linear_acceleration_covariance
        imu_msg.angular_velocity = best_gyro.angular_velocity
        imu_msg.angular_velocity_covariance = best_gyro.angular_velocity_covariance

        # orientation: 기본값(항등 쿼터니언)으로 채움!
        imu_msg.orientation.x = 0.0
        imu_msg.orientation.y = 0.0
        imu_msg.orientation.z = 0.0
        imu_msg.orientation.w = 1.0
        # covariance: 값이 없으면 0~0.01 정도 넣어도 무방
        imu_msg.orientation_covariance[0] = 0.01

        self.imu_pub.publish(imu_msg)

    def to_stamp(self, msg):
        return msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

def main(args=None):
    rclpy.init(args=args)
    node = ImuCombiner()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

 

 

 

3. packge.xml에 추가,

  <exec_depend>rclpy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>

 

 

4. setup.py

            'combine_imu = imu_tools.combine_imu:main',

 

 

 

 

5. 빌드 및 소싱

cd ~/ros2_ws_imu
colcon build
source install/setup.bash

 

 

 

 

6.

ros2 launch realsense2_camera rs_launch.py     enable_gyro:=true     enable_accel:=true     unite_imu_method:=linear_interpolation

 

 

7.

ros2 run imu_tools combine_imu

 

 

 

8.

ros2 launch rtabmap_launch rtabmap.launch.py   rtabmap_args:="--delete_db_on_start"   rgb_topic:=/camera/camera/color/image_raw   depth_topic:=/camera/camera/depth/image_rect_raw   camera_info_topic:=/camera/camera/color/camera_info imu_topic:=/imu/data   approx_sync:=True

 

 

 

9.

  ros2 run tf2_ros static_transform_publisher \
  	0 0 0 -1.5708 0 -1.5708 \
    base_link \
    camera_color_optical_frame