ROS2 nav2(4)

오도메트리 설정을 해보겠다.

오도메트리 시스템은 로봇의 움직임에 따라 로봇의 자세와 속도에 대한 정확한 추정치를 제공한다. 오도메트리 정보는 IMU, LIDAR, RADAR, VIO 및 휠 인코더와 같은 다양한 소스에서 얻을 수 있다. 한 가지 주의할 점은 IMU는 시간이 지남에 따라

드리프트하는 반면 휠 인코더는 이동 거리에 따라 드리프트하므로 종종 서로의 부정적인 특성을 상쇄하기 위해 함께 사용된다.

 

odom프레임과 이와 관련된 변환은 로봇의 오도메트리 시스템을 사용하여 시간이나 거리에 따라 정확도가 떨어지지만 연속적인 현지화 정보를 게시한다. (센서 모달리티 및 드리프트에 따라 다름). 그럼에도 불구하고 로봇은 여전히 이 정보를 사용하여 바로 주변을 탐색할 수 있다.(예: 충돌 회피)

시간이 지남에 따라 일관되게 정확한 오도메트리 정보를 얻기 위해 map프레임은 odom프레임을 수정하는 데 사용되는 전역적으로 정확한 정보를 제공한다.

 

odom프레임은 odom => base_link 변환을 통해 나머지 시스템과 nav2에 연결된다. 이 변환은 tf2 브로드캐스터 또는 추가 기능을 제공하는 robot_localization과 같은 프레임워크들로 퍼블리시 된다.

 

odom => base_link 변환 외에도 nav2는 nav_msgs/Odometry 메세지 퍼블리시가 필요하다. 이 메시지는 로봇의 속도를

제공하기 때문이다.

nav_msgs/Odometry 메시지에는 다음 정보가 들어있다.

# This represents estimates of position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id
# The twist in this message should be specified in the coordinate frame given by the child_frame_id

# Includes the frame id of the pose parent.
std_msgs/Header header

# Frame id the pose is pointing at. The twist is in this coordinate frame.
string child_frame_id

# Estimated pose that is typically relative to a fixed world frame.
geometry_msgs/PoseWithCovariance pose

# Estimated linear and angular velocity relative to child_frame_id.
geometry_msgs/TwistWithCovariance twist

이 메시지는 로봇의 포즈와 속도에 대한 추정치를 알려준다.

header 메시지는 주어진 좌표 프레임에서 타임스탬프가 찍힌 데이터를 제공한다.

pose 메시지는 header.frame_id에서 지정된 프레임에 대한 로봇의 위치와 방향을 제공한다.

twist 메시지는 child_frame_id에서 정의된 프레임에 대한 선형 및 각속도를 제공한다.

 

Gazebo를 사용하여 오도메트리 시스템 시뮬레이션

먼저 gazebo와 ros2에서 작동하도록 하는 데 필요한 패키지를 설정한다. 다음으로, IMU 센서와 차동 구동 오도메트리 시스템을 시뮬레이션하는 gazebo 플러그인을 추가하여 각각 sensor_msgs/Imu와 nav_msgs/Odometry 메시지를 퍼블리시 한다. 마지막으로 sam_bot을 가제보 환경에 스폰하고 퍼블리시된 sensor_msgs/Imu와 nav_msgs/Odometry 메시지를 확인할 것이다.

 

이제까지 URDF로 해왔다 그런데 gazebo는 SDF파일을 사용한다. 하지만 다행히도 가제보는 URDF파일을 자동으로 SDF파일로 바꾸어준다. gazebo는 각 <link>마다 <inertia>를 가지고 있는데 이것은 저번 시간에 작성을 이미 했다.

 

URDF에 Gazebo 플러그인 추가

IMU센서와 Gazebo의 차동 드라이브 플러그인을 URDF에 추가하겠다.

로봇의 경우 SensorPlugin인 GazeboRosImuSensor를 사용할 것이다. SensorPlugin은 링크에 연결되어야 하므로 IMU 센서가 연결될 imu_link를 만들 것이다. 이 링크는 <gazebo> 요소 아래로 설정될 것이다. 다음으로 /demo/imu를 IMU가 정보를 게시할 토픽으로 설정하고 initalOrientationAsReference to false 세팅함으로써 REP145를 준수한다.

또 gazebo의 센서 노이즈 모델을 사용하여 센서 구성에 약간의 노이즈를 추가한다.

 

IMU센서 플러그인을 추가해보겠다. </robot>전에 아래 내용을 추가한다.

<link name="imu_link">
  <visual>
    <geometry>
      <box size="0.1 0.1 0.1"/>
    </geometry>
  </visual>

  <collision>
    <geometry>
      <box size="0.1 0.1 0.1"/>
    </geometry>
  </collision>

  <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>
</link>

<joint name="imu_joint" type="fixed">
  <parent link="base_link"/>
  <child link="imu_link"/>
  <origin xyz="0 0 0.01"/>
</joint>

 <gazebo reference="imu_link">
  <sensor name="imu_sensor" type="imu">
   <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
      <ros>
        <namespace>/demo</namespace>
        <remapping>~/out:=imu</remapping>
      </ros>
      <initial_orientation_as_reference>false</initial_orientation_as_reference>
    </plugin>
    <always_on>true</always_on>
    <update_rate>100</update_rate>
    <visualize>true</visualize>
    <imu>
      <angular_velocity>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </z>
      </angular_velocity>
      <linear_acceleration>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </z>
      </linear_acceleration>
    </imu>
  </sensor>
</gazebo>

 

이제 차동 구동 모델 플러그인을 추가해보겠다. 플러그인을 구성하여 /demo/odom토픽에  nav_msgs/Odometry메시지가 퍼블리시 되도록한다. 아래 내용을 IMU플러그인 뒤에 추가한다.

<gazebo>
  <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
    <ros>
      <namespace>/demo</namespace>
    </ros>

    <!-- wheels -->
    <left_joint>drivewhl_l_joint</left_joint>
    <right_joint>drivewhl_r_joint</right_joint>

    <!-- kinematics -->
    <wheel_separation>0.4</wheel_separation>
    <wheel_diameter>0.2</wheel_diameter>

    <!-- limits -->
    <max_wheel_torque>20</max_wheel_torque>
    <max_wheel_acceleration>1.0</max_wheel_acceleration>

    <!-- output -->
    <publish_odom>true</publish_odom>
    <publish_odom_tf>false</publish_odom_tf>
    <publish_wheel_tf>true</publish_wheel_tf>

    <odometry_frame>odom</odometry_frame>
    <robot_base_frame>base_link</robot_base_frame>
  </plugin>
</gazebo>

 

이제 gazebo에서 스폰되도록 저번에 만들었던 display.launch.py를 편집한다.

로봇을 시뮬레이션 할 것이므로 아래 내용을 삭제하여 조인트 상태 퍼블리셔의 gui를 제거할 수 있다.

joint_state_publisher_gui_node = launch_ros.actions.Node(
  package='joint_state_publisher_gui',
  executable='joint_state_publisher_gui',
  name='joint_state_publisher_gui',
  condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
)

 

다음 GUI 매개변수도 제거한다.

DeclareLaunchArgument(name='gui', default_value='True',
                      description='Flag to enable joint_state_publisher_gui')

 

조건과 매개변수를 제거하고 joint_state_publisher_node에 인수를 추가한다.

joint_state_publisher_node = launch_ros.actions.Node(
  package='joint_state_publisher',
  executable='joint_state_publisher',
  name='joint_state_publisher',
  arguments=[default_model_path], #Add this line
  parameters=[{'robot_description': Command(['xarcro ', default_model_path])}], #Remove this line
  condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui')) # Remove this line
)

 

package.xml을 열고 아래 내용도 삭제한다.

<exec_depend>joint_state_publisher_gui</exec_depend>

 

gazebo를 시작하려면 다음 joint_state_publisher_node줄을 추가한다. (display.launch.py)

launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'], output='screen'),

 

return launch.LaunchDescription([ 전 줄에 아래 내용도 추가한다.

spawn_entity = launch_ros.actions.Node(
  package='gazebo_ros',
  executable='spawn_entity.py',
  arguments=['-entity', 'sam_bot', '-topic', 'robot_description'],
  output='screen'
)

 

그 다음 rviz_node 전에 spawn_entity, 라인에 아래 내용 추가한다.

      robot_state_publisher_node,
      spawn_entity,
      rviz_node
])

 

프로젝트의 루트로 이동하여 다음 줄을 실행한다

colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.py

 

Gazebo가 실행되고 아래와 같은 모델이 표시된다.

 

ros2 topic info /demo/imu
Type: sensor_msgs/msg/Imu
Publisher count: 1
Subscription count: 0

ros2 topic info /demo/odom
Type: nav_msgs/msg/Odometry
Publisher count: 1
Subscription count: 0

위와 같은 결과가 나오면 된다.

 

robot localization demo

robot_localization 패키지는 오도메트리 센서 입력에서 제공하는 데이터로부터 융합되고 정확하고 매끄러운 오도메트리 정보를 제공하는데 사용된다.

이 정보들은 nav_msgs/Odometry, sensor_msgs/Imu, geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped 메시지로부터 제공받을수있다.

 

일반적인 로봇 설정은 최소한 휠 인코더와 IMU를 오도메트리 센서 소스로 구성한다. 여러 소스가 robot_localization에 제공되면

상태 추정 노드를 사용하여 센서에게 제공하는 오도메트리 정보를 융합할 수 있다. 이러한 노드는 확장 칼만 필터(ekf_node) 또는

무향 칼만 필터(ukf_node)를 사용하여 이 융합을 구현한다. 또한 이 패키지는 GPS로 작업할 때 지리적 좌표를 로봇의

세계 프레임으로 변환하는 navsat_transform_node를 구현한다.

 

robot_localization 패키지에서 융합된 센서 데이터는 odometry/filtered 및 accel/filtered 토픽을 통해 게시되며 (활성화 된 경우)

/tf 토픽에서 odom => base_link 변환을 퍼블리시 할 수도 있다.

 

만약 로봇이 제공할 수 있는 오도메트리 정보가 하나만 있다면, robot_localization 패키지를 사용해도 주로 smoothing외에는 큰 효과를 기대하기 어렵다. 이경우 대안으로 단일 오도메트리 소스 노드에서 tf2 브로드캐스터를 사용하여 변환을 발행하는 방법이 있다. 그럼에도 불구하고, robot_localization을 사용하여 변환을 발행하도록 선택할 수 있으며, 이 경우 출력에서 일부 smoothing 효과를 관찰할 수 있다.

 

이제 robot_localization 패키지를 구성하여 확장 칼만 필터(ekf_node)를 사용해서 오도메트리 정보를 융합하고

odom => base_link 변환을 퍼블리시 해보겠다.

 

먼저 robot_localization을 설치해준다

sudo apt install ros-<ros2-distro>-robot-localization

 

다음으로 yaml파일을 사용하여 매개변수를 지정한다. 프로젝트 루트에 config폴더를 만들고 ekf.yaml파일을 만든다.

그리고 아래 내용을 복사하여 붙여준다.

### ekf config file ###
ekf_filter_node:
    ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of theinputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
        frequency: 30.0

# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
        two_d_mode: false

# Whether to publish the acceleration state. Defaults to false if unspecified.
        publish_acceleration: true

# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.
        publish_tf: true

# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
#    to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
#    observations) then:
#     3a. Set your "world_frame" to your map_frame value
#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
#         from robot_localization! However, that instance should *not* fuse the global data.
        map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_link  # Defaults to "base_link" ifunspecified
        world_frame: odom           # Defaults to the value ofodom_frame if unspecified

        odom0: demo/odom
        odom0_config: [true,  true,  true,
                       false, false, false,
                       false, false, false,
                       false, false, true,
                       false, false, false]

        imu0: demo/imu
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      false, false, false,
                      false, false, false]

 

우리는 nav_msgs/Odometry와 sensor_msgs/Imu를 필터에 대한 입력으로 가지고 있고 odom0과 imu0을 사용한다.

odom0의 값으로 demo/odom(nav_msgs/Odometry 를 퍼블리시하는 토픽) 비슷하게 imu0의 값으로 demo/Imu(sensor_msgs/Imu 를 퍼블리시하는 토픽)을 설정했다.

 

매개변수를 사용하여 필터에서 사용할 센서의 값을 지정할 수 있다. 이 매개변수 값의 순서는 x,y,z,roll,pitch,yaw,vx,vy,vz,vroll,vpitch,vyaw,ax,ay,az입니다. odom0_config에서 1,2,3,12번째 항목을 제외한 모든 항목을 false로 설정했는데 이는 필터가 odom0의 x,y,z,vyaw만 사용한다는 것을 의미한다.

 

마찬가지로 imu0_config에서는 roll,pitch,yaw만 사용된다는 것을 알 수 있다. 일반적인 모바일 로봇용 IMU는 각속도와 선형 가속도도 제공한다. 하지만 robot_localization이 제대로 작동하려면, 서로 유도되는 파라미터들을 융합해서는 안된다.

예를 들어 각속도는 IMU 내부에서 roll,pitch,yaw를 추정하기 위해 융합되므로, 이 정보를 바탕으로 파생되는 각속도를 다시 융합하면 안된다.

 

특히 고급 IMU를 사용하는 것이 아니라면 각속도는 일반적으로 노이즈가 많이 생기기 때문에 robot_localization에 사용하지 않는 것이 좋다.

 

이제 launch 파일에 ekf_node를 추가해 보겠다.  launch/display.launch.py를 열고 return launch.LaunchDecription([ 전에 아래 내용을 추가한다.

robot_localization_node = launch_ros.actions.Node(
       package='robot_localization',
       executable='ekf_node',
       name='ekf_filter_node',
       output='screen',
       parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}]
)

 

다음으로 return launch.LaunchDescription([ 내부에 아래 내용을 추가한다.

launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',
                                            description='Flag to enable use_sim_time'),

 

마지막으로 robot_localization_node를 실행하기 위해 rviz_node위에  robot_localization_node,을 추가해준다

      robot_state_publisher_node,
      spawn_entity,
      robot_localization_node,
      rviz_node
])

 

다음으로 패키지에 종속성을 추가해줘야 하는데 package.xml을 열고 <exec_depend>마지막 줄에 아래 내용을 추가해준다.

<exec_depend>robot_localization</exec_depend>

 

또 CMakeLists.txt에 install(DIRECTORY...) 부분에 config도 추가해준다.

install(
  DIRECTORY src launch rviz config
  DESTINATION share/${PROJECT_NAME}
)

 

이제 빌드 및 실행을 해본다

colcon build
source install/setup.bash
ros2 launch sam_bot_description display.launch.py

 

위와 같은 결과를 얻을 수 있다.

 

아까와 같이 아래 코드를 실행해보면 subscriber가 하나씩 늘었다는 것을 알 수 있다.

ros2 topic info /demo/imu
Publisher count: 1
Subscription count: 1

ros2 topic info /demo/odom
Publisher count: 1
Subscription count: 1

 

ekf_filter_node도 살펴보면 아래와 같은 내용이 나오는데 /demo/imu와 /demo/odom이 subscriber인 것을 확인할 수있다.

ros2 node info /ekf_filter_node

/ekf_filter_node
  Subscribers:
    /clock: rosgraph_msgs/msg/Clock
    /demo/imu: sensor_msgs/msg/Imu
    /demo/odom: nav_msgs/msg/Odometry
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /set_pose: geometry_msgs/msg/PoseWithCovarianceStamped
  Publishers:
    /accel/filtered: geometry_msgs/msg/AccelWithCovarianceStamped
    /diagnostics: diagnostic_msgs/msg/DiagnosticArray
    /odometry/filtered: nav_msgs/msg/Odometry
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /ekf_filter_node/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /ekf_filter_node/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /ekf_filter_node/get_parameters: rcl_interfaces/srv/GetParameters
    /ekf_filter_node/list_parameters: rcl_interfaces/srv/ListParameters
    /ekf_filter_node/set_parameters: rcl_interfaces/srv/SetParameters
    /ekf_filter_node/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
    /enable: std_srvs/srv/Empty
    /set_pose: robot_localization/srv/SetPose
    /toggle: robot_localization/srv/ToggleFilterProcessing
  Service Clients:

  Action Servers:

  Action Clients:

 

tf2_echo 유틸리티를 사용하여 robot_localization 패키지가 odom => base_link 변환을 퍼블리시 하고 있는지 확인할 수 있다.

ros2 run tf2_ros tf2_echo odom base_link

[INFO] [1730867093.853976044] [tf2_echo]: Waiting for transform odom ->  base_link: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist
At time 148.737000000
- Translation: [-0.221, -0.005, 0.243]
- Rotation: in Quaternion [0.000, -0.000, 0.006, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.012]
- Rotation: in RPY (degree) [0.000, -0.000, 0.707]
- Matrix:
  1.000 -0.012 -0.000 -0.221
  0.012  1.000 -0.000 -0.005
  0.000  0.000  1.000  0.243
  0.000  0.000  0.000  1.000
At time 149.737000000
- Translation: [-0.222, -0.005, 0.245]
- Rotation: in Quaternion [0.000, -0.000, 0.006, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.012]
- Rotation: in RPY (degree) [0.000, -0.000, 0.710]
- Matrix:
  1.000 -0.012 -0.000 -0.222
  0.012  1.000 -0.000 -0.005
  0.000  0.000  1.000  0.245
  0.000  0.000  0.000  1.000
.
.
.