cjh1995-ros.log

NVIDIA Omniverse Basic Environment setup

Mon, 11 Jul 2022 15:57:28 GMT

NVIDIA Omniverse basic interfaces

Mon, 11 Jul 2022 13:08:26 GMT

NVIDIA Omniverse를 다룰 때 다른 시뮬레이터와 다른 키를 사용하여 조종한다.

기본 Viewport 조작

기본 마우스 조작 을 다음과 같이 알아보자.

left click
right click
wheel
wheel + move

alt + left click
alt + right click
object 위에 올린 후 F 누르기.

기본 물체 제어

기본적인 물체인 cube, corn 등은 상단의 create -> shape 에서 찾을 수 있다.

E/R/W/WW/Esc 기본적인 물체를 변형할 때 사용된다. W는 물체를 움직일 때, E는 물체를 회전시킬 때, R은 물체의 크기를 변경시킬 때 사용된다. WW는 물체를 Local axes 방향으로 움직일 때 사용된다. 아래는 각각을 순서대로 사용했을 때이다. Esc는 선택한 물체를 해제하는 것을 의미한다.

Property Panel 사용 앞서 말한 내용을 Property Panel을 사용해서 제어할 수도 있다.
Stage widget 사용 stage widget에서 물체를 tree 구조로 연결할 수도 있다.

참고자료

isaac docs

Install NVIDIA SDK Manager in Ubuntu 20.04

Mon, 02 May 2022 07:00:42 GMT

작년까지의 기억으로는 Ubuntu 20.04에는 SDK Manager를 설치 못하는 불편함이 있었던 걸로 기억하는데, 이제는 아닌 듯 하다. 하지만 참고를 위해서 다음과 같은 링크를 정리해두었다.

Reference

설치해도 안됐을 때

되는거 고려한 링크

Install NVIDIA VPI for Elbrus SLAM

Mon, 02 May 2022 06:09:22 GMT

1. What is VPI?

VPI란 NVIDIA에서 제공하는 라이브러리로 Vision Programming Interface를 의미한다. ~~마치 OpenCV를 대체하기 위해 만든 라이브러리 처럼 느껴진다...~~ 특정 기능들은 nvidia jetson 시리즈에서만 사용이 가능하다.

1 PVA, VIC and NVENC backends are only available on Jetson devices, such as Jetson AGX Orin, Jetson AGX Xavier and Jetson Xavier NX. 2 OFA backend is only available on Jetson AGX Orin devices.

특히 좋은 장점은 지원되는 device에 한해 zero-copy memory 기능이다. 나는 이 라이브러리를 NVIDIA의 Elbrus SLAM를 사용하기 위해 설치한다. 현재 나와있는 것은 2.0 버전의 공식 docs인데 이 두 가지(1, 2)를 살펴보면1.1.11 버전을 요구한다.

Install Error

이 에러로 인해 계속해서 빌드가 막히고 있다.

Solution

해당 링크에서 답을 찾을 수 있었다. 즉 SDK manager를 설치했어야지만 이를 사용할 수 있다는 것이다. 이는 현재 내 컴퓨터에서는 불가능함으로 컴퓨터 재설치가 불가하다.

2. Install Isaac Packages in ROS2_ws

해당 링크에서 관련 내용을 자세히 찾을 수 있다.

Nvidia Omniverse (Isaac SIM)

Fri, 11 Feb 2022 14:49:09 GMT

시뮬레이션이니까 world 위의 모든 object의 위치를 파악할 수 있다.

Task 이해하기

말 그대로 일을 주는 것이다. 로봇팔이라면 pick&place가 될 것이고, 주행로봇이면 A2B의 일을 하는 것이다. 이러한 명령들이 모여모여 하나의 시스템이 될 터이니, 가장 간단한 Task를 이해하며 늘려가보자.

1. Task Class 이해하기

[task]

set_up_scene(): World에서 로봇이나 world를 추가하듯이, 여기서도 동일하게 작동할 수 있다. 즉 이 함수 안에서 객체 삽입하라는 것.
get_observations(): 삽입된 객체의 정보들을 관찰할 수 있다. return 이 존재하며, dictionary형태로 돌려주어야 한다.
pre_step(): 각각의 물리스텝 전에 실행이 되는 함수이다. 이곳에서 일의 성공 유무를 파악하면 되겠다.

2. World Class에서 task 활용하기.

삽입된 객체는 scene을 통해서 확인할 수 있다. 실제 움직이는 것은 world의 add_physics_callback 함수를 통해서 움직여야 함으로,

world.add_task(Task(name="task1")): 해야할 task를 삽입할 수 있다.
get_observations() 함수를 사용하여 필요한 관찰 데이터 획득.

필요한 데이터를 습득했으면, 그곳으로 이동하는 함수를 사용하여 task를 진행하면 되겠다.

from omni.isaac.examples.base_sample import BaseSample
from omni.isaac.franka import Franka
from omni.isaac.jetbot import Jetbot
from omni.isaac.core.objects import DynamicCuboid
from omni.isaac.core.controllers import BaseController
from omni.isaac.franka.controllers import PickPlaceController
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.tasks import BaseTask

import numpy as np


class FrankaPlaying(BaseTask):
    def __init__(self, name):
        super().__init__(name=name, offset=None)
        self._goal_position=np.array([-30, -30, 5.15/2.])
        self._task_achieved  = False

    # Here we setup all the assets that we care about in this task.
    def set_up_scene(self, scene):
        super().set_up_scene(scene)
        scene.add_default_ground_plane()
        self._cube = scene.add(
            DynamicCuboid(
                prim_path="/World/random_cube",
                name="fancy_cube",
                position=np.array([30, 30, 30]),
                size=np.array([5.15, 5.15, 5.15]),
                color=np.array([1., 0, 0])
            )
        )
        self._franka = scene.add(
            Franka(
                prim_path="/World/Fancy_Franka",
                name="fancy_franka"
            )
        )
        return

    # Information exposed to solve the task is returned from the task through get_observations
    def get_observations(self):
        cube_position, _ = self._cube.get_world_pose()
        current_joint_positions = self._franka.get_joint_positions()
        observations = {
            self._franka.name: {
                "joint_positions": current_joint_positions,
            },
            self._cube.name: {
                "position": cube_position,
                "goal_position": self._goal_position
            }
        }
        return observations

    # Called before each physics step,
    # for instance we can check here if the task was accomplished by
    # changing the color of the cube once its accomplished
    def pre_step(self, control_index, simulation_time):
        cube_position, _ = self._cube.get_world_pose()
        if not self._task_achieved and np.mean(np.abs(self._goal_position - cube_position)) < 2:
            # Visual Materials are applied by default to the cube
            # in this case the cube has a visual material of type
            # PreviewSurface, we can set its color once the target is reached.
            self._cube.get_applied_visual_material().set_color(color=np.array([0, 1.0, 0]))
            self._task_achieved = True
        return

class HelloWorld(BaseSample):
    def __init__(self) -> None:
        super().__init__()
        return

    def setup_scene(self):
        world = self.get_world()
        # We add the task to the world here
        world.add_task(FrankaPlaying(name="my_first_task"))
        return

    async def setup_post_load(self):
        self._world = self.get_world()
        # The world already called the setup_scene from the task (with first reset of the world)
        # so we can retrieve the task objects
        self._franka = self._world.scene.get_object("fancy_franka")
        self._controller = PickPlaceController(
            name="pick_place_controller",
            gripper_dof_indices=self._franka.gripper.dof_indices,
            robot_prim_path=self._franka.prim_path,
        )
        self._world.add_physics_callback("sim_step", callback_fn=self.physics_step)
        await self._world.play_async()
        return

    async def setup_post_reset(self):
        self._controller.reset()
        await self._world.play_async()
        return

    def physics_step(self, step_size):
        # Gets all the tasks observations
        current_observations = self._world.get_observations()
        actions = self._controller.forward(
            picking_position=current_observations["fancy_cube"]["position"],
            placing_position=current_observations["fancy_cube"]["goal_position"],
            current_joint_positions=current_observations["fancy_franka"]["joint_positions"],
        )
        self._franka.apply_action(actions)
        if self._controller.is_done():
            self._world.pause()
        return

Nvidia Omniverse (Isaac SDK + SIM)

Thu, 10 Feb 2022 03:08:47 GMT

omniverse isaac sim과 isaac sdk를 연결해서 진행해보려고 한다. 튜토리얼을 살펴보자. 도커에서 다음과 같이 치자

bob@desktop:~/isaac/sdk$ bazel run apps/navsim/navsim_navigate -- --map_json apps/assets/maps/virtual_test_warehouse_1.json

또한 omniverse에서 omni:/Isaac/Samples/Isaac_SDK/Scenario/str_dolly_warehouse_with_forklifts.usd를 찾아서 실행해보자.

에러중 다음과 같이 20.04에러가 나오긴 하지만 로봇은 제대로 돌아갔다. 해서 이부분은 문제가 없다고 생각하자.

2022-02-10 11:00:36.536 ERROR external/com_nvidia_isaac_engine/engine/alice/components/Codelet.cpp@229: Component '_check_operating_system/isaac.alice.CheckOperatingSystem' of type 'isaac::alice::CheckOperatingSystem' reported FAILURE:

    Isaac requires Ubuntu 18.04 while the host has Ubuntu 20.04.

2022-02-10 11:00:36.536 ERROR external/com_nvidia_isaac_engine/engine/alice/backend/event_manager.cpp@43: Stopping node '_check_operating_system' because it reached status 'FAILURE'

치명적 단점

Isaac sdk는 RTX 30xx를 지원하지 않는다. 해서 이러한 부분들은 해결 가능하지 못하니 본인의 그래픽카드를 새로구매하던가, 아니면 deep learning을 사용하지 않으면 되겠다.

[ref]

isaac sdk with omniverse isaac sdk, [isaac sdk docs]
omniverse isaac sdk bridge,i.e bridge extensions, [omniverse docs]
isaac sdk cuda error, [cuda version error]
Isaac Jetbot Sample, [jetbot sample]
Isaac training in docker, [training sample]

Nvidia Omniverse (Isaac SDK)

Thu, 10 Feb 2022 01:39:05 GMT

Isaac samples

setup

Prerequisites (os 버전 제외하고는 완전 필수는 아닌듯)

Recommendation

	OS	Graphic Card	Cuda
Desktop	Ubuntu 18.04 LTS	>440	>6.1
Robot	Jetpack4.5.1	..	..

Mine

	OS	Graphic Card	Cuda
Desktop	Ubuntu 20.04 LTS(docker)	470	11.4
Robot	Jetpack4.5.1	..	..

sdk download

https://developer.nvidia.com/isaac-sdk

Installing Dependencies on Desktop

Desktop

bob@desktop:~/isaac/engine/$ ./engine/build/scripts/install_dependencies.sh

Robot dependencies를 재밌게도 host pc에서 로봇에 원격설치해주는 방식을 사용한다. ssh가 설치되어있어야 하니 주의할 것.
```
bob@desktop:~/isaac/engine/$ ./engine/build/scripts/install_dependencies_jetson.sh -u  -h 
```

Docker 다행히 도커 옵션을 주면 도커에서 로봇으로 ssh를 통한 접속이 가능하다. 해서 사용에 완전 문제가 없다.

bob@desktop:~/isaac/sdk$ ../engine/engine/build/docker/install_docker.sh
bob@desktop:~/isaac$ ./engine/engine/build/docker/create_image.sh
bob@desktop:~/isaac$ docker volume create isaac-sdk-build-cache
bob@desktop:~/isaac$ docker run --mount source=isaac-sdk-build-cache,target=/root -v `pwd`:`pwd` -w `pwd`/sdk --runtime=nvidia -it isaacbuild:latest /bin/bash

기본 사용법

bazel을 사용해서 패키지를 관리하고 배포한다. 나는 도커를 사용해서 필요한 버전을 쉽게 설치했지만 다른분들은 모르겠다. 다른건 모르겠고 3.1버전만 된다는 것을 기억하자.

bazel로 패키지를 관리하는 법은 간단하다.

build

# 전체 패키지 빌드
bazel build ...

하나만 빌드

bob@desktop:~/isaac/sdk$ bazel build //engine/gems/filters/examples:ekf_sin_exp

전체 테스트

bob@desktop:~/isaac/sdk$ bazel test ... --jobs=1

하나만 테스트

bob@desktop:~/isaac/sdk$ bazel test //engine/gems/optimization/tests:pso_ackley


- run
빌드한 파일 실행하기
```shell
bob@desktop:~/isaac/sdk$ bazel run //engine/gems/filters/examples:ekf_sin_exp

deploy 빌드한 파일 로봇에 배포하는 법이다. 이때 로봇에 ssh 연결이 꼭 되어있어야한다.

desktop에서 배포.

# app 배포
bob@desktop:~/isaac/sdk$ ./../engine/engine/build/deploy.sh --remote_user  -p //apps/samples/stereo_dummy:stereo_dummy-pkg -d jetpack45 -h 
# package 배포
bob@desktop:~/isaac/sdk$ ./../engine/engine/build/deploy.sh --remote_user  -p //packages/visual_slam/apps:elbrus_visual_slam_zed-pkg  -d jetpack45 -h

run in robot 빌드한 파일 로봇에서 사용하기

# 로봇에 ssh를 통한 접속
# app 실행 
bob@jetson:~/$ cd deploy/bob/stereo_dummy-pkg
bob@jetson:~/deploy/bob/stereo_dummy-pkg$ ./apps/samples/stereo_dummy/stereo_dummy
# package 실행
bob@jetson:~/$ cd deploy/bob/
bob@jetson:~/deploy/bob$ ./packages/visual_slam/apps/elbrus_visual_slam_zed

sample 사용

도커에서 필요한 옵션은 다음과 같다. myshell.sh

#!/bin/bash
docker run \
    --mount source=isaac-sdk-build-cache,target=/root \
    -v `pwd`:`pwd` \
    -w `pwd`/sdk \
    --network host \
    -v /dev:/dev \
    --rm \
    --name isaac-zed \
    --gpus all \
    -it \
    isaac-zed:latest \
    /bin/bash

물론 전체 /dev를 할 필요는 없다. -v /dev:/dev 를 -device /dev/video0 같이 바꾸어도 똑같이 된다. 아쉬운 점은 웹캠은 쉽게 설치가 되었지만 zed sdk나 realsense sdk 를 쉽게 설치할 순 없었다. 그들만의 도커를 사용해야하는데 이것은 해결못하였다.

웹캠을 사용한다면 본인 웹캠의 스펙에 맞는 내용을 적어주도록하자.

v4l2_camera.app.json 중 config

"config": {
"camera": {
  "V4L2Camera": {
    "device_id": 2,
    "rows": 480,
    "cols": 640,
    "rate_hz": 30
  }
},
"websight": {
  "WebsightServer": {
    "port": 3000,
    "ui_config": {
      "windows": {
        "Camera": {
          "renderer": "2d",
          "channels": [
            { "name": "v4l2_camera/viewer/ImageViewer/image" }
          ]
        }
      }
    }
  }
}

rows와 cols에 지원하는 내용을 적어주도록 하자. 그리고 json파일을 사용하니 다시 빌드할 필요없이 그 내용만 수정하면 되겠다.

Run Jupyter notebook

isaac sdk의 데이터를 보고 싶어서 파이썬 파일을 작성해야하는지, 어떤걸 해야하는지 고민하다가 tutorial에 jupyter notebook을 사용할 수 있다고 하여 쓴 내용이다.

docker에서 다음과 같이 실행하면 에러가 나온다
```
bazel run //apps/tutorials/jupyter_notebook:jupyter_notebook
```

[C 09:29:08.480 NotebookApp] Running as root is not recommended. Use --allow-root to bypass.

docker 컨테이너 안이라면 사용에 arg를 다음과 같이 넣어줘야 한다.
```
bazel run //apps/tutorials/jupyter_notebook:jupyter_notebook -- --allow-root
```

참고자료 : bazel args

SLAM and DL Paper Lists

Mon, 07 Feb 2022 11:51:36 GMT

Lidar SLAM

Cartographer - Real-time SLAM in 2D and 3D across multiple platforms and sensor configurations [github cartographer]
FAST-LIO - efficient and robust LiDAR-inertial odometry package [github FAST-LIO]
LOL - Lidar-only Odometry and Localization in 3D point cloud maps [github lol]
PyICP SLAM: Full-python LiDAR SLAM using ICP and Scan Context [github PyICP SLAM]
LIO-SAM - Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping [github LIO-SAM]
LeGO-LOAM - Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain [github LeGO-LOAM]
hdl_graph_slam - 3D LIDAR-based Graph SLAM [github hdl_graph_slam]
A-LOAM - Advanced implementation of LOAM [github A-LOAM]
LIO-mapping - A Tightly Coupled 3D Lidar and Inertial Odometry and Mapping Approach [github LIO-mapping]
SC-LeGO-LOAM - LiDAR SLAM: Scan Context + LeGO-LOAM [github SC-LeGO-LOAM]
Fast LOAM: Fast and Optimized Lidar Odometry And Mapping for indoor/outdoor localization [github Fast LOAM]
LINS: LiDAR-inertial-SLAM [github LINS]

VSLAM

AprilSAM - Real-time smoothing and mapping [github xipengwang/AprilSAM]
DSO - Novel direct and sparse formulation for Visual Odometry [github dso]
ElasticFusion - Real-time dense visual SLAM system [github ElasticFusion]
fiducials - Simultaneous localization and mapping using fiducial markers [github UbiquityRobotics/fiducials]
GTSAM - Smoothing and mapping (SAM) in robotics and vision [github borglab/gtsam]
Kintinuous - Real-time large scale dense visual SLAM system [github Kintinuous]
LSD-SLAM - Real-time monocular SLAM [github lsdslam]
ORB-SLAM2 - Real-time SLAM library for Monocular, Stereo and RGB-D cameras [github ORB_SLAM2]
ORB-SLAM3 - Visual, Visual-Inertial and Multi-Map SLAM [github ORB_SLAM3]
RTAP-Map - RGB-D Graph SLAM approach based on a global Bayesian loop closure detector [github introlab/rtabmap]
SRBA - Solving SLAM/BA in relative coordinates with flexibility for different submapping strategies [github srba]
DeepFactor - Real-Time Probabilistic Dense Monocular SLAM [github DeepFactor]
ProSLAM - ProSLAM uses the lightning-fast, header-only HBST library for binary descriptor similarity search (loop closing) [github ProSlam]
Vins-Mono - A Robust and Versatile Monocular Visual-Inertial State Estimator [github Vins-Mono]
OpenVSLAM - A Versatile Visual SLAM Framework [github OpenVSLAM]
Semantic SLAM - Semantic SLAM can generate a 3D voxel based semantic map using only a hand held RGB-D camera in real time [github Semantic Slam]
ROVIO - Robust Visual Inertial Odometry [github ROVIO]
LIFT-SLAM - a deep-learning feature-based monocular visual SLAM method [Paper LIFT-SLAM]
SVO-SLAM - Fast Semi-Direct Monocular Visual Odometry [github SVO]
MonoSLAM - real-time single camera SLAM [Paper MonoSLAM]
CodeSLAM - Pytorch base SLAM [github CodeSLAM]
Elbrus Stereo VSLAM - Elbrus is based on two core technologies: Visual Odometry (VO) and Simultaneous Localization and Mapping (SLAM). [nvidia link]

Object Detection

Yolov4 - It is fast, easy to install, and supports CPU and GPU computation [github yolov4]
DETR - End-to-End Object Detection with Transformers [github DETR]
Hydranets - The HydraNet is a neural network architecture that splits into multiple branches (or heads) close to the top of the network [github hydranets]
CAM2BEV - A sim2real deep learning approach for the transformation of images from multiple vehicle-mounted camera to a semantically segmented image in bird's eye view [github cam2bev]

Depth estimation

monodepth-1 - Unsupervised single image depth prediction [github monodepth1]
monodepth-2 - self-supervised learning has emerged as a promising alternative for training models to perform monocular depth estimation. [github monodepth2]
keras simple example [keras example]

Dataset

SLAM datasets [Awesome-SLAM-datasets]

Awesome

NVIDIA Omniverse 시작 - 00

Sat, 05 Feb 2022 16:42:10 GMT

최근 sim2real 기법으로 딥러닝의 부족한 데이터를 채우는 모습을 확인할 수 있다. 나 또한 이러한 기법에 깊게 감명받아 3d design과 simulation을 공부하려고 한다.

먼저 3d design 툴은 어떤 것들이 있을까? Autodesk 사의 제품들이 우리가 주로 사용하는 것이고, 공대생이라면 creo, cartia, solid works 등이 더 익숙할 것이다. 예체능 사람들에게는 blender, 3d max 등이 익숙하다.

그 다음 위의 3d 툴로 만든 것들을 simulation 해보고 싶을때, 어떠한 플랫폼을 사용할까? unity, unreal, gazebo, carla 등이 있다.

각각의 장단점은 존재한다. 나같은 경우에는 gazebo만 사용해봤으며 unity, unreal은 사용이 어려워 중도 하차를 했다. 하지만 최근 sim2real 기법이 대세로 자리잡으며 다시한번 simulation 툴을 익히겠다는 다짐을 하였다.

Nvidia omniverse라고 들어는 보았나? 비교적 최근에 나온 플랫폼이며 정확한 실시간 시뮬레이션과 협업이 가능한 툴이다. 아래는 nvidia korea에서 설명한 내용이다.

NVIDIA Omniverse™는 가상 협업 및 물리적으로 정확한 실시간 시뮬레이션을 위해 구축된 개방형 플랫폼입니다. 크리에이터, 디자이너, 연구자 및 엔지니어는 주요 설계 도구, 자산 및 프로젝트를 연결하여 공유 가상 공간에서 협업하고 과정을 반복할 수 있습니다. 개발자와 소프트웨어 제공업체는 Omniverse의 모듈식 플랫폼에서 확장 프로그램, 앱, 커넥터 및 마이크로 서비스를 쉽게 구축 및 판매하여 기능을 확장할 수도 있습니다.

핵심 특징은 다음과 같다.

사용자와 애플리케이션 간의 실시간 협업이 가능
실시간 속도. 오프라인 품질
NVIDIA RTX 기술로 렌더링된 사실적인 이미지를 모든 디바이스에서 스트리밍 가능
2D -> 3D workflow 가속

두번째는 영문 버전으로 nvidia omniverse를 설명한 링크이다. 여기서 내가 주의 깊게 본것은 nvidia omniverse가 따로 사용하는 데이터 형식 USD이다.

Universal Scene Description (USD) is an easily extensible, open-source 3D scene description and file format developed by Pixar for content creation and interchange among different tools. As a result of its power and versatility, it’s being widely adopted, not only in the visual effects community, but also in architecture, design, robotics, manufacturing, and other disciplines. Omniverse uses USD for interchange through the Nucleus DB service.

~~로봇쪽에서도 채택하는 데이터 형식이라하니, 이제 sdf를 보내주고 usd를 사용하는 omniverse의 세계로 가보자...~~

Omniverse는 다음 5가지 핵심 component를 갖고 있다.

Nucleus
Connect
Kit
Simulation
RTX Renderer

Omniverse consists of 5 key parts: Nucleus, Connect, Kit, Simulation, and RTX Renderer. These components, along with interoperable third-party digital content creation (DCC) tools and renderers - and third-party and NVIDIA-built extensions, apps, and microservices make up the full Omniverse ecosystem.

즉 타사 제품과 화합하기 위해 사용되는 툴이라는 것. 각각에 대해 알아보자.

Nucleus

Connect

Kit

Simulation

RTX Renderer

Nvidia Create

Western Housing Assets

[ref]

ROS1, ROS2, Conda 같이 쓰기

Wed, 02 Feb 2022 05:16:52 GMT

1. ros1 설치하기

http://wiki.ros.org/noetic/Installation/Ubuntu

2. ros2 설치하기

https://docs.ros.org/en/foxy/Installation.html

3. conda 설치하기

https://www.anaconda.com/products/individual

4. 셋다 alias로 실행하기

https://stackoverflow.com/questions/54429210/how-do-i-prevent-conda-from-activating-the-base-environment-by-default

아나콘다 자동실행 방지로 다음과 같이 ~/.bashrc에 작성한다.

export CONDA_AUTO_ACTIVATE_BASE=false
alias do_foxy='source /opt/ros/foxy/setup.bash; source ~/cjh_ws/ros2_ws/install/local_setup.bash; echo "Activate foxy!"'
alias do_noetic='source /opt/ros/noetic/setup.bash; source ~/cjh_ws/ros1_ws/devel/setup.bash; echo "Activate noetic"'
alias do_conda='conda activate base'

ROS2 RVIZ Marker

Sat, 29 Jan 2022 04:21:47 GMT

rviz2에서 마커를 본 경우가 많을 것이다. 나중에 사람이나 다른 객체의 이동 데이터를 얻으면 마커를 옮기는 방식으로 표현하고 싶어서 마커를 표시하는 방법을 찾아봤다. 메시지는 다음과 같다.

visualization_msgs

위 메시지를 이용해서 데이터를 pub 하면 된다. ros wiki에서 위 메시지를 찾아보자.

uint8 ARROW=0
uint8 CUBE=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 LINE_STRIP=4
uint8 LINE_LIST=5
uint8 CUBE_LIST=6
uint8 SPHERE_LIST=7
uint8 POINTS=8
uint8 TEXT_VIEW_FACING=9
uint8 MESH_RESOURCE=10
uint8 TRIANGLE_LIST=11
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3
std_msgs/Header header
string ns
int32 id
int32 type
int32 action
geometry_msgs/Pose pose
geometry_msgs/Vector3 scale
std_msgs/ColorRGBA color
duration lifetime
bool frame_locked
geometry_msgs/Point[] points
std_msgs/ColorRGBA[] colors
string text
string mesh_resource
bool mesh_use_embedded_materials

Marker안에 있는 속성들이다. 이를 다음과 같이 활용한다.

#include 
#include "rclcpp/rclcpp.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include 
#include 
#include 

using namespace std::chrono_literals;

class MarkerPub: public rclcpp::Node
{
private:
  rclcpp::Publisher::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  void markerCB()
  {
    auto marker = visualization_msgs::msg::Marker();
    marker.header.frame_id = "base_link";
    marker.header.stamp = this->now();
    marker.ns = "cjh";
    marker.id = 0;
    marker.type = visualization_msgs::msg::Marker::CUBE;
    marker.action =  visualization_msgs::msg::Marker::ADD;
    marker.pose.position.x = 1;
    marker.pose.position.y = 1;
    marker.pose.position.z = 1;
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;
    marker.color.a = 1.0;
    marker.color.r = 0.0;
    marker.color.g = 1.0;
    marker.color.b = 0.0;
    marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
    publisher_->publish(marker);
  }

public:
  MarkerPub():Node("marker_test")
  {
    publisher_ = this->create_publisher("marker", 10);
    timer_ = this->create_wall_timer(
      20ms, std::bind(&MarkerPub::markerCB, this)
    );
  }
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared());
    rclcpp::shutdown();


    return 0;
}

후에 frame_id는 human정도로 해서 넣어줄까 생각중이다. 또한 ns는 namespace로 동일 마커간 구분을 위해 사용한다. 이외에도 다양한 마커 종류가 있으니 자신에게 맞게끔 활용하면 되겠다.

[ref]

ROS2 Gazebo

Wed, 26 Jan 2022 10:04:14 GMT

1. WareHouse 제작

(1) 링크의 내용 중 아마존 aws에서 제작한 warehouse model을 사용하기로 했다. 다른 모델들 대부분은 ROS2를 지원하지 않았고, 이는 수정이 오래걸리는 것으로 예상되기에 채택하지 않았다.

설치방법

# go to your ros2_ws/src
git clone -b foxy-devel https://github.com/aws-robotics/aws-robomaker-small-warehouse-world.git
cd ..
rosws update
rosdep install --from-paths src --ignore-src -r -y
colcon build
# 만약 ~/.bashrc안에 아래의 명령어 넣어놨으면 그걸 사용
source install/setup.bash
ros2 launch aws_robomaker_small_warehouse_world no_roof_small_warehouse_launch.py

성공시 위와 같은 사진이 나온다.

2. Turtlebot3 불러오기

위 gazebo world에 turtlebot3를 불러오기 전에, turtlebot3_simulation을 설치하자.

git clone -b foxy-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
rosdep install --from-paths src --ignore-src -r -y
colcon build
source ~/.bashrc
ros2 launch turtlebot3_gazebo empty_world.launch.py

성공하면 위와 같은 가제보를 볼 수 있다. warehouse파일과 turtlebot 파일이 확실히 되는 것을 알았으니, 다음과 같이 파일을 수정해보자.

1. 'src/aws-robomakers-small-warehouse-world/worlds/no_roof_small_warehouse/no_roof_small_warehouse.world' 마지막에 다음과 같은 코드를 삽입한다.


  -2.0 -0.5 0.01 0.0 0.0 0.0
  model://turtlebot3_waffle

2. 'src/aws-robomakers-small-warehouse-world/models/' 안에 turtlebot3_waffle 혹은 burger 모델을 넣는다. 
위같이 하고 아까와 동일하게 파일을 launch하면 이번에는 터틀봇이 있는 상태로 존재한다.

만일 불안하다면 복사한 파일에서 실행하도록 하자.

3. RVIZ에서 보기

위에서 실행한 상태로 RVIZ를 키고 세팅하도록 하자. 그러면 다음과 같은 에러가 주루루루룩 나올 거다. 게다가 tf를 보면 map과 odom만 나올 것이다. 즉 우리는 로봇의 tf를 publish하지 않은 것이다(물론 이상태로도 turtlebot을 움직이는 것은 가능). 정말 고맙게도 우리는 robot_state_publisher를 사용하여 이를 해결할 수 있을 것이다. 특히 로보티스 사에서 launch 파일에 robot_state_publisher를 만들어 주었으니 이를 활용하도록 하자.

즉 우리는 2가지를 launch해야한다. 한가지는 aws패키지, 다른 것은 robot_state_publisher이다. 다음과 같은 launch 파일을 만들도록 하자. turtlebot3_warehouse.launch.py

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess, DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.conditions import IfCondition


TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
WORLD_MODEL = os.environ['WAREHOUSE_MODEL']

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='True')

    aws_small_warehouse_dir = os.path.join(get_package_share_directory('aws_robomaker_small_warehouse_world'), 'launch')

    if WORLD_MODEL == 'small_warehouse':
        aws_small_warehouse = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([aws_small_warehouse_dir, '/no_roof_small_warehouse_launch.py'])
        )

    elif WORLD_MODEL == 'big_warehouse':
        aws_small_warehouse = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([aws_small_warehouse_dir, '/big_warehouse_launch.py'])
        )


    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
    robot_state_publisher = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([launch_file_dir, '/robot_state_publisher.launch.py']),
            launch_arguments={'use_sim_time': use_sim_time}.items(),
    )

    ld = LaunchDescription()
    ld.add_action(aws_small_warehouse)
    ld.add_action(robot_state_publisher)

    return ld

이후 빌드하고 실행하면 다음과 같이 rviz로도 내용을 확인할 수 있다.

4. Intel Realsense Gazebo Plugin 사용

위에서 한 것들을 좀 더 자세히 생각해보자. 우리는 urdf와 sdf를 정말 이해하고 있는가? 난 이제야 쪼~오금 더 이해가 간다. urdf는 기본적으로 tf 중심으로 짜여져 있으며, 거기서 topic이 나오지 않는다. robot_state_publisher로 tf관계도만 나온다는 것이다. 그렇다면 gazebo 상의 센서값들은 우리가 어디서 보는 것일까? 바로 sdf파일에 작성되어 있는 것들로 본다. 그러므로 만일 우리가 realsense gazebo plugin을 사용하고 싶다? urdf파일도 수정해야 하고 sdf파일도 수정해야 한다는 것과 같다. sdf와 urdf의 차이점은 (2)에서 설명이 되어있다. 요약하면 다음과 같다.

urdf = Unified Robotic Description Format
sdf = Simulation Description Format
why sdf?

URDF can only specify the kinematic and dynamic properties of a single robot in isolation. URDF can not specify the pose of the robot itself within a world. It is also not a universal description format since it cannot specify joint loops (parallel linkages), and it lacks friction and other properties. Additionally, it cannot specify things that are not robots, such as lights, heightmaps, etc.

~~결과적으로 가상환경에 알맞는 형식이 아니라고 한다. 그래서 gazebo는 sdf형식의 파일을 만들어서 사용한다고 한다.~~

결론 = rviz에서는 tf형식만! gazebo에서 plugin을 이용한 topic이 나온다.

~~위의 결론을 보고 나니 우리는 urdf와 sdf를 비교할 필요가 있다. 그래야 위에서 다운로드 받은 intel realsense d435i plugin을 사용할 수 있지 않겠나?~~

urdf vs sdf

어떤 tag가 사용가능/대체/추가 되는지 간단하게 알아보자.

Required

 태그 안에는 가 꼭 있어야 한다.

Optional

*  안에  element를 넣는 것.
  - color 를 gazebo format으로 바꿀 수 있음
  - Add sensor plugins
* Add a  element for every 
  - Set proper damping dynamics
  - Add actuator control plugins

적당히 알아보는데, 더 알아보고 싶다면 (2)에서 더 알아보자.

아니 그래서 sdf가 필요한 이유는 알겠는데 그래서 어떻게 realsense를 가제보 상에서 사용할건데? 가제보에 올라가는 것은 결국엔 model.sdf파일이니, 이부분을 수정해보도록하자.

top-level launch = turtlebot3_warehouse.launch.py second-level launch = no_roof_small_warehouse_launch.py, robot_state_publisher.launch.py no_roof_small_warehouse_launch.py = gzserver, gzclient, waffle model.sdf robot_state_publisher.launch.py = urdf parse.

즉 waffle의 model.sdf파일, turtlebot3_waffle_d435i.urdf를 사용함. 둘을 수정해야 한다는 뜻.

더 찾아보니 urdf를 gazebo로 spawn하는 방법이 있다. 단 inertia와 플러그인 다른 방식을 통해 작성한다. 예시는 아래와 같다.

이때 xacro, xml macro 같은 것을 통해 간단하게 표시해주자.

4. d435i Plugin 포기.

포기 이유.

urdf로 작성된 파일을 sdf로 작성해줘야하는 수고로움이 너무 큼.
만약 ros2의 rtab-map을 맛보고 싶은거라면, depth camera plugin을 사용하면 됨. (4)에서 kinect카메라를 사용함. --> libDepthCameraPlugin.so로 바뀜. 다양한 plugin을 확인하고 싶으면 아래의 폴더에서 확인하자.

/usr/lib/x86_64-linux-gnu/gazebo-11/plugins

isaac sim으로 빠른 시일안에 넘어갈 것.

gazebo는 navigation 파라미터 수정용으로만 사용해보자.

기본 plugin 활용

최종적인 화면이다. 사용법은 정말 간단했다. camera 타입을 바꿔주는 것. 아래와 같다.

camera.sdf

camera.urdf

  
    
      true
      30.0
      
        1.047198
        
          640
          480
          R8G8B8
        
        
          0.05
          3
        
      
      
        0.2
        true
        0.0
        camera_depth_frame
        0.5
        3.0
        0
        0
        0
        0
        0
        0
        0
        0
        0
        0

바뀐 부분은 다음과 같다.

type의 camera가 depth로 바뀜.
depth frame
pointCloudCutoff+Max

위와 같은 내용들이 추가되었다. distortion이나 focalLength는 기본적인 것이니 굳이 넣으라 말라 하지 않겠다.

5. Google Cartographer

현재 중요한 문제가 있다. 계속해서 map과 odom 사이의 transform이 생성되지 않아 로봇이나 scan msg가 맵상에서 보이지 않고 있다. 웃기는건 cartographer노드에서 나오는 토픽은 다 된다는 점. 하지만 여전히 TF에러가 있다. 어디서 잘못된건지 파악이 필요하다.

6. RTAB-Map

먼저 rtabmap을 lidar로만 만든 형식이다. 어제 만든 urdf가 계속해서 문제를 일으키고 있다. 정확히는 urdf가 아니라 sdf파일이 굉장히 까다로운 부분이 있는 것 같다. 특히 위에서 map과 odom사이의 transform이 안나와서 뭔가 안되는 것을 보면 이부분이 굉장히 스트레스다. 정작 tf_tree를 확인하면 문제는 없고... 아직은 갈길이 먼것 같다. 찾아보니 tf tree의 문제는 없었다. 단순히 gazebo가 에러를 일으켰던 것. 어쨋든 위와 같이 pcl을 이용한 mapping을 하였더니 아래와 같은 map을 구했다. 본인 카메라의 fov나 focal length같은 것을 고려해서 다시 해보자.

튜닝의 끝은 순정이라는 말을 믿고 기본 튜토리얼을 실행해보자. 아래는 cartographer로 만든 맵이다. 해당 맵을 기준으로 navigation을 한 모습이다. 도착까지 잘 한다.

Feed back

로봇의 어떤 움직임이 loop closure에 방해가 될까 -> 지속적인 좌우 회전 혹은 제자리 회전이 로봇 맵 전체적으로 영향을 주었다.
urdf 작성 방식 -> xacro 를 적극적으로 사용해보자. 혹은 stl파일을 사용하는 것도 좋다.

[ref]

ROS2 realsense

Mon, 24 Jan 2022 02:45:17 GMT

ROS2 Realsense Wrapper Install

ros2의 realsense wrapper를 다운로드 받아야 한다. 먼저 본인이 현재 쓰고 있는 realsense sdk의 버전을 모른다면 필히 알아보도록 하자. 모른다면 realsense-viewer를 키면 되겠다.

검색을 해보니 다음과 같은 링크가 나온다.

https://github.com/intel/ros2_intel_realsense 하지만 확인해보면 dashing 까지만 지원해주고 foxy같은 경우에는 설치가 안된다. 그러므로 다음과 같이 정공법으로 승부한다.
https://github.com/IntelRealSense/librealsense/releases

위 링크에서 본인의 버전에 맞는 것을 살펴본다. 맞는 버전의 내용에서 ROS(Learn More) 이라고 적혀있는 것을 들어가면 다음과 같은 화면이 나온다. 이곳에서 원하는 버전을 찾고, 소스코드를 다운받아 본인의 ros2_ws에서 풀면 되겠다. 그 후 dependency나 빌드 방식은 처음의 링크와 동일한 방식으로 해줘도 무방한듯 하다. 해서 다음과 같은 shell파일을 만들었다. 만일 본인의 버전이 나와 다르다면 알아서 능력껏 찾아서 설치하면 되겠다.

#!/bin/bash
INSTALL_DIR=$PWD
source /opt/ros/foxy/setup.bash
cd /usr/include
sudo ln -s opencv4 opencv
cd $INSTALL_DIR

DEFAULTDIR=cjh_ws/ros2_ws
CLDIR="$1"
if [ ! -z "$CLDIR" ]; then 
 DEFAULTDIR="$CLDIR"
fi
# Check to see if qualified path already
if [ -d "$DEFAULTDIR" ] ; then
   echo "Fully qualified path"
else
   # Have to add path qualification
   DEFAULTDIR=$HOME/$DEFAULTDIR
fi
echo "DEFAULTDIR: $DEFAULTDIR"


# install dependencies
sudo apt-get install ros-foxy-cv-bridge ros-foxy-librealsense2 ros-foxy-message-filters ros-foxy-image-transport
sudo apt-get install -y libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev
sudo apt-get install -y libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev

if [ "${DEFAULTDIR: -1}" != "/" ] ; then
    DEFAULTDIR=$DEFAULTDIR/
fi

INSTALLDIR="$DEFAULTDIR"src
if [ -e "$INSTALLDIR" ] ; then
  echo "Installing realsense-ros in: $INSTALLDIR"
else
  echo "$INSTALLDIR does not appear to be a source of a Catkin Workspace"
  echo "The source directory src does not exist"
  echo "Terminating Installation"
  exit 1
fi 

echo "Starting installation of realsense-ros"

cd $INSTALLDIR

# Update the dependencies database
rosdep update
echo "Cloning Intel ROS realsense package"
# Prerequisite: ddynamic_reconfigure
# git clone https://github.com/pal-robotics/ddynamic_reconfigure
wget https://github.com/IntelRealSense/realsense-ros/archive/refs/tags/3.2.2.tar.gz
tar -xvf 3.2.2.tar.gz
echo "Unzip 3.2.2.tar.gz folder!"
echo "remove zip folder and go to build file"
rm -rf 3.2.2.tar.gz
cd ..

# Remove the librealsense2 package requirement in package.xml
# patch -p1 < $INSTALL_DIR/patches/package.diff

echo $PWD
echo "Making Intel ROS realsense-ros"

# catkin_make -> colcon build
colcon build --base-paths src/ros2_intel_realsense

설치가 완료되었다면 다음과 같은 명령어를 실행해보자.

ros2 launch realsense2_camera demo_pointcloud_launch.py

그러면 다음과 같은 화면이 나올것이다.

그러면 설치는 완성한 것!

Realsense d435i calibration

d435i에서 imu 데이터를 사용하기 위해선 imu값을 넣어줘야 한다. intel camera는 좋은 점이 calibration데이터를 밖에서 들고있어야 하는것이 아닌, 카메라 내부에 저장이 된다. 반대로 않좋은 점은 calibration 값을 넣어주는 방식이 따로 존재한다는 것. 그래서 왠만하면 intel realsense에서 제공하는 파일을 사용하는 것이 속편하다. 하지만 camera intrinsic mat calibration은 ubuntu 16.04, 18.04에서만 제공함으로 사용하기 까다롭다. 그러니 camera intrinsic은 제외하도록 하자.

rs-imu-calibration.py 파일을 검색하여 실행하자. 여기서 중요한 점이 있다. 본인이 ubuntu 20.04를 사용한다면, 기본적으로 python3로 실행이 될텐데, 나는 Python3로 실행했을 때 다음과 같은 에러가 계속해서 나왔다.

failed to set power state

본래 guide line에서는 Python2로 실행하니, 동일하게 버전 맞추어 python2에 pyrealsense2를 설치해주어 실행시켰다. 해서 Ubuntu 20.04에 python2를 설치하는 법을 보자.

sudo apt update
sudo apt install python2.7
sudo apt install curl
sudo add-apt-repository universe
curl https://bootstrap.pypa.io/pip/2.7/get-pip.py --output get-pip.py # Fetch get-pip.py for python 2.7 
python2 get-pip.py
pip --version
pip install numpy
pip install enum34
pip install pyrealsense2
python rs_calibration_imu.py

위 명령어를 실행하면 이제 카메라를 요리조리 돌려서 모양을 맞추면 되겠다. 만일 본인이 이상한 usb 선을 사용한다면 위에서 나온 에러를 볼 수 있다. 꼭 usb 3를 사용하도록 하자.

결과를 카메라에 저장했다면 realsense-viewer에서 다음과 같이 motion module을 볼 수 있다.

Realsense d435i launch

ros2 launch realsense2_camera rs_launch.py

위의 명령어만 실행하면 카메라 pcl 과 imu를 볼 수 없다. 그러니 다음과 같은 옵션을 넣어주도록 하자.

ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true enable_pointcloud:=true

혹은 본인이 launch file을 수정해서 사용해도 되겠다.

import copy
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os
import sys
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
import rs_launch


local_parameters = [{'name': 'enable_pointcloud', 'default': 'true', 'description': 'enable pointcloud'},
        {'name': 'enable_gyro', 'default':'true', 'description':'enable gyro'},
            {'name': 'enable_accel', 'default': 'true', 'description':'enable_accel'},
                   ]

def set_configurable_parameters(local_params):
    return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params])

def generate_launch_description():
    rviz_config_dir = os.path.join(get_package_share_directory('realsense2_camera'), 'rviz', 'pointcloud.rviz')
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output = 'screen',
        arguments=['-d', rviz_config_dir],
        parameters=[{'use_sim_time': False}]
        )


    return LaunchDescription(
        rs_launch.declare_configurable_parameters(local_parameters) + 
        [
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']),
                launch_arguments=rs_launch.set_configurable_parameters(local_parameters).items(),
                ),
            rviz_node
        ])

ROS URDF to SDF

Sun, 23 Jan 2022 15:12:07 GMT

URDF란?

Unified Robot Description Format으로 ROS1에서 자주 사용하던 형식이다.

SDF란?

Simulation Description Format으로 urdf와 조금 다른 형식을 취한다.

어떻게 바꾸나?

xml urdf to pure urdf

xacro my_urdf_with_xml.urdf > my_urdf_without_xml.urdf

pure urdf to sdf

gz sdf -p my_urdf_without_xml.urdf > my_sdf.sdf

변환 성능은? ???? 그렇게 좋지는 않은듯. 아무래도 sdf로 바꿔야 하는 상황 아니면 꼭 하진 않을듯. 가제보에서는 이미 urdf로 사용하는 법을 제공한다!

[ref]

3. Hanwhabot 만들기

Tue, 18 Jan 2022 04:11:28 GMT

한화봇은 다음과 같이 구성되어 있다.

sensor - Intel realsense d435i, YDlidar X4, mpu6050 actuator - MD750T MCU - arduino mega board - Jetson nano main github link

https://github.com/cjh1995-ros/hanwhabot.git

1. MD750T Control

먼저 로봇 구동부를 살펴보자. 터틀봇은 OpenCR을 통해 로봇을 구동하고 있다. 그렇다면 OpenCR 내부에서는 어떤 코드가 작동하고 있을까? https://github.com/ROBOTIS-GIT/OpenCR/tree/master/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_burger/turtlebot3_core 참고해서 보면 되겠지만, 정확히 모터랑 serial 통신을 하는 모습은 보이지 않는다. 하지만 모터 값을 다루는 코드는 확인할 수 있는데, 특별히 odometry를 계산하는 방법이 다른것도 아니다. 그렇다면 나 또한 MD750T를 다룰 때, 동일한 방식으로 하면 되겠다라는 생각했다.

1-0. MD750T ROS src, manual 다운로드 링크

http://www.mdrobot.co.kr/tech-support/?q=YToyOntzOjEyOiJrZXl3b3JkX3R5cGUiO3M6MzoiYWxsIjtzOjc6ImtleXdvcmQiO3M6MzoiUk9TIjt9&bmode=view&idx=6051407&t=board 버전이 계속해서 바뀌는 것 같은데, 실험을 통해서 현재 작성자와 같은 버전인지 확인할 필요 있다. 확인 내용은 odometry값이다.

1-1. MD750T 통신

먼저 MD750T와의 통신이다. 정말 많은 통신을 고려했지만, RS232 통신이 가장 간단하여 그 통신을 채택하였다. RS232 통신을 할때는 md에서 수정해줘야 할 것이 몇가지 없다. 먼저 launchfile의 baudrate를 9600으로 해주고 com.cpp 파일의 38번째 줄을 다음과 같이 수정해주었다.

serial::Timeout to = serial::Timeout::simpleTimeout(1000);

이후에 RS232 통신 성공했다.

1-2. MD750T odometry

하지만 MD750T의 코드는 문제가 없지만, 막상 확인을 해보면 전혀 다른 odom 값을 돌려주고 있다. 정면으로 가면 ROS 좌표계상 x좌표 값이 증가해야하는데, y값이 증가를 한다거나, theta는 deg값을 돌려주는 일이 발생했다. 사실 좌표축이 바뀌면서 값 자체에 대한 신뢰도가 떨어졌지만, 전화상담을 통해 얘기해보니, 절대값에는 문제가 없는 것으로 나온것 같다. 그렇다면 나는 어떻게 문제를 해결했나? 다음과 같이 cmd_main.cpp에서 odom.pose, odom.twist를 결정해주었다.

typedef struct RightOdom{
  double posx, posy, posth;
  double velx, angz;
} rightOdom;
rightOdom r_odom;

void monitorCallBack(const md::monitor_msg::ConstPtr& monitor)
{
  ~
  MD.lPosiX = monitor->lPosiX;
  MD.lPosiY = monitor->lPosiY;
  // my custom, x, y 바뀜. 180도 회전 정도로 생각하자.
  r_odom.posx = (double)-monitor->lPosiY/1000; // mm -> m
  r_odom.posy = (double)-monitor->lPosiX/1000; // mm -> m
  r_odom.posth = monitor->sTheta ? (double)(360-(float)(monitor->sTheta/10))*3.141592/180.0 : 0;
  r_odom.velx = (double)-monitor->sRealLinearVel/1000; // mm/s -> m/s
  r_odom.abgz = (double)(-monitor->sReanAngularVel/10.0*3.141592/180.0); // 10deg/sec -> rad/sec
  //  
}

ros의 angular 은 +x축을 0으로 잡고 왼쪽 으로 가며 -x에 도달했을 때 180, 반대 방향으로 출발했을 때는 -180이다. 하지만 +z로 회전하여 x축에 도달하여도, 360으로 취급하여도 상관이 없다. 하지만 만일 본인이 생각하기엔 다른 방식의 각도를 생각하고 싶다면 위 방식 말고 다음과 같이 선택하여도 상관없는 듯 하다. 실험결과 동일하였다.

r_odom.posth = MD.fTheta-90 ; // 이후에 각도 바꾸기.

이후에는 동일하게 odom을 publish 해주면 되겠다. 관련 코드 링크는 아래에 내 깃헙에 올려두었다. 이외에 수정해야할 파라미터는 launch 파일에 다 있다. Manual과 함께 읽어보면 알겠지만, PC, MDUI, MDT, RMID값은 꼭 확인하길 바란다. 왜냐면 중간에 다른 하드웨어를 사용한다면 방금 값들이 바뀌기 때문이다. 그 다음 본인 모터에 해당하는 값들을 바꿔주도록 하자.

2. YDLidar X4

YDLidar는 저가 2d 라이다로 학생들이 구매하기 좋은 제품이라 생각한다. 사용법 또한 간단하나 아쉬운점은 생각보다 센서값이 꽉차지 못했다는 것. +x 정면값이 laser[360]이다. 사용법은 아래에 간단히 적어놓겠다.

2-1. YDLidar X4 ros 설치

1) Clone this project to your catkin's workspace src folder
    (1). git clone https://github.com/YDLIDAR/ydlidar_ros
    (2). git chectout master
2) Running catkin_make to build ydlidar_node and ydlidar_client
3) Create the name "/dev/ydlidar" for YDLIDAR
--$ roscd ydlidar_ros/startup
--$ sudo chmod 777 ./*
--$ sudo sh initenv.sh

위 같은 방식을 통해 설치가 되었다면(udev rule 또한 설치됨), launch 폴더에 가서 본인에게 맞는 lidar를 실행해보자. 정말 다양한 라이다가 존재한다.

3. Intel Realsense d435i

Intel RealSense 설치는 정말 힘들지만 JetsonHack라는 분이 정말 쉽게 만들어놓으신 것들이 많다. 나 또한 OpenCV나 ROS설치는 여기서 작성한 스크립트를 사용한다.

3-1. Intel Realsense d435i install

여기서 중요한 것은 버전이다. 너무 낮은 버전을 사용하면 그건 그거나름대로 실행이 안된다. 너무 높은 버전 또한 ROS와 호환이 안될 수 있다. 적당한 버전을 찾아서 설치해야하는데, 어떻게 할까? 먼저 다음의 링크에서 확인을 하자.

위 링크에서 버전을 확인하는데, 너무 높은 버전은 피하자. 2022-01-18일 최신 버전은 2.50.v인데, 내가 설치할때 당시만 해도 2.48이 최신버전이였다. 하지만 내가 생각하기에 안전한 버전은 2.45.0v라 생각하고 그것에 맞는 ros 버전을 찾아준다. 찾아보니 2.3.0 버전이다. 필요한 버전을 파악했으니, 이제 jetsonhacks의 버전을 바꿔주자. 확인하면 두가지 실행파일이 존재하는데, installLibrealsense.sh를 하면 가장 최신버전이 설치되니 build로 가자.

$ ./buildLibrealsense.sh [ -v | --version  ] [ -j | -jobs  ] [ -n | --no_cuda ]

위와 같은 방식으로 진행된다는데, 우리는 cuda도 존재하고 하니 버전만 잘 설정해주면 된다.

$ ./buildLibrealsense.sh -v v2.45.0

설치가 다 되었다면, 다음은 ros wrapper설치이다. 설치 스크립트 중 13번째 줄을 본인의 버전에 맞게 바꿔주면 되겠다.

REALSENSE_ROS_VERSION=2.2.11 # -> 2.3.0

4. mpu6050 ros

중간에 계단을 오르는 미션이 있어서 탱크가 계단을 다 올랐는지 아닌지에 대한 판단을 하기위해 imu 센서를 사용하기로 했다.

4-1. mpu6050 ros install

mpu6050 ros의 경우 다음과 같은 링크를 참고하였다.

https://automaticaddison.com/visualize-imu-data-using-the-mpu6050-ros-and-jetson-nano/

위 링크의 Set Up the Communication Protocol 까지만 실행 한 뒤 바로 아래에 내려가서 아래의 깃헙을 사용하는 링크로 가자. 그 후 실행되는지 확인한 후 설치를 마치면 되겠다. https://github.com/bandasaikrishna/orientations_from_IMU_MPU_6050.git

ROS1의 navigation은 참고자료가 정말 많다.

ROS1 navigation stack은 다음과 같다. 여기서 만약 본인 만의 path planning을 만드는 것이 아니라, 말 그대로 자신의 로봇에 ROS navigation을 사용하는 방법에 대해 알고 싶다면 다음과 같은 calibration을 해줘야 한다.

odometry calibration
amcl calibration
dwa path planner calibration

위의 calibration을 하기 전에 일단 로봇의 차체가 완성되었다는 전제하에 움직여야 한다. 그렇다는 것은 로봇의 tf가 다 나와있어야 한다는 것인데, 이는 어떻게 해결할 수 있는가? tf에 대한 이해는 http://wiki.ros.org/tf 이 링크에서 하고, 나는 실질적으로 어떻게 tf broadcasting을 쉽게 하는지만 얘기하고 싶다. 링크에서의 tutorial은 tf를 굉장히 불편하게 broadcasting하고 있다. 물론 tf 변환이 필요하다면 그 의미가 다르겠지만, 만일 본인이 navigation에 필요한 tf 변환만 필요하다면, 다음과 같이 간단하게 사용하자.

urdf 파일 작성
launch 파일 작성

본인이 만일 urdf를 작성할 줄 모른다면 다음의 링크를 확인해서 작성하자. http://wiki.ros.org/urdf 여기서 재밌는 점은, turtlebot이나 다른 판매하는 로봇들은 제대로된 urdf를 작성했지만, 나는 그렇게 하진 못했다. 캐터필러의 정확한 stl 파일 확보를 못했을 뿐더러, 이 과정자체도 정말 어려운 것이기에 편법을 사용했다. 정말로 navigation stack에서 요구하는 urdf만 작성을 한 것이다. 그 다음 launch file에서 urdf parser를 사용해서 urdf를 tf broadcasting 해주는 것을 사용하면 되겠다. 관련 파일은 깃헙 링크, hanwhabot_description 폴더에서 찾아볼 수 있다. 위에서 odom_combined를 odom으로만 바꾸어서 tf tree를 만들면 되겠다. 참고로 odom부분은 위의 md에서 만들어줘야 한다. urdf는 로봇에 대한 것만(정적인 것만) 작성하는 것처럼 보인다.

tf가 세팅되었다면 다음은 calibration이다. 그 중 첫번째로 해야되는 것은 odometry calibration이다. 그게 어떤 의미일까?

5-1. odometry calibration

보통 두가지로 나뉜다. 한가지는 x축 이동에 대한 calibration. 다른 한가지는 theta에 대한 calibration이다. 전자에 대해 설명하자면, 로봇이 정면으로 움직였을 때, 로봇에게 1m를 움직이라 하였을 때, 실제 1m를 움직였는가? 실제로는 0.9m정도 움직였을 것이다. 어차피 추후에 filter를 거칠것이긴 하지만, 어느정도 정확해야지 filter값이 작아질 것이고, 작은 값일 수록 navigation을 돌리는데 정확해진다. 어떤 filter값인지는 아래에서 설명할 것이다.

5-1-1. x calibration

간단하다. 실제 움직인 거리를 확인하면 되겠다. 만일 그 값이 어느정도 정확하다 싶으면 그 다음은 lidar calibration을 하면 되겠다. lidar calibration이란, 전방의 벽에 라이다를 키면서 앞으로 나아가면, lidar frame 기준으로는 laser pcl이 frame으로 가깝게 올 것이다. 그것이 실제로 되는지 rviz 상에서 확인하면 되겠다.

5-1-2. angular calibration

제자리 회전을 한 후 나오는 odometry 값과 실제 회전각을 비교하면 되겠다. 나같은 경우에는 이부분에서 굉장히 애를 먹었는데, 캐터필러를 사용하다 보니 kinematics가 정확히 나오지 않아 2wheel 모델을 그대로 사용했다. 그 부분 때문에 md.launch 파일의 wheelLength 값을 계속해서 바꾸었다. 실제로 확인했을 때 360 회전을 했는데, 계산상으로는 320도 혹은 380도 이런식을 나오는 것을 수정하라는 것이다. 더 정확히 하고 싶다면 실제로 n바퀴 + a를 회전하고 그것의 계산 값을 비교하면서 calibration을 하라는 것이다. 이는 정형화된 방법은 없을 것이다. 각자에게 맞는 방식으로 calibration을 하면 되겠다.

5-2. amcl calibration

amcl이란 laser기반으로 현재 pose를 추정하는 패키지이다. 이중에는 위에서 odometry calibration은 하였지만, 그래도 완벽하게 동일하진 않을 것이다. 일단 launch 파일과 링크를 통해 각각의 parameter가 어떤 것을 의미하는 지 알아보자. http://wiki.ros.org/amcl

5-2-1. amcl odom parameter calibration

여기서 odometry에 관한 filter 설정 파라미터를 알아보자. odom_alpha1~4까지 있는데, 이 값들이 필터값이다. 각각에 대해 이해해보자.

odom_alpha1: 로봇의 회전으로 인한 rotation odometry noise 필터
odom_alpha2: 로봇의 translation으로 인한 rotation odometry noise 필터
odom_alpha3: 로봇의 회전으로 인한 translation odometry noise 필터
odom_alpha4: 로봇의 translation 인한 translation odometry noise 필터

우리팀의 로봇은 특히 회전값이 안맞았으므로 odom alpha1, 2값을 크게 주었다(0.4, 0.4). 하지만 마냥 크게 주면 그거때문에 또 값을 제대로 인식하지 못한다. 적절한 값을 선정하는 실험을 거치도록 하자.

5-2-2. amcl laser parameter calibration

그 다음은 laser에 대한 calibration이다. 먼저 라이다 부착 위치와 주변 프레임으로 인해 lidar의 범위가 상당히 좁아졌다. 이는 내 개인 github에서 확인할 수 있다. ~~lidar launch file 넣기~~

좁아진 범위를 생각한다면 laser max beams 파라미터를 바꿔야 함을 알 수 있다.

laser_max_beams: How many evenly-spaced beams in each scan to be used when updating the filter

즉 필터 업데이트시 사용할 수 있는 beams의 개수인데, 현재 값이 180으로 매우 높은 수준이다. 그러므로 X4.launch 파일에서 설정한 각도의 절반 값을 주자.

5-3. DWA Path planner calibration

http://wiki.ros.org/navigation/Tutorials/RobotSetup movebase.launch 파일을 통해 어떤 파일을 수정해줘야 하는지 확인해보자. 모든 파라미터에 대해 알아 보진 않을 것이다. 먼저 move_base.launch 파일을 보면
costmap_common_params.yaml
global_costmap_params.yaml
local_costmap_params.yaml
dwa_local_planner_params.yaml

각 폴더의 내용을 보자.

5-3-1. costmap_common_params.yaml

수정해줘야 하는 값들은 다음과 같다. footprint, inflation radius, cost_scaling_factor

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
#robot_radius: ir_of_robot
inflation_radius: 0.55

observation_sources: laser_scan_sensor point_cloud_sensor

laser_scan_sensor: {sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking: true, clearing: true}

point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}

obstacle_range

The "obstacle_range" parameter determines the maximum range sensor reading that will result in an obstacle being put into the costmap

2.5가 의미하는건 2.5m 내부의 값들만 장애물로 costmap에 넣을 것이라는 뜻이다.

raytrace_range

The "raytrace_range" parameter determines the range to which we will raytrace freespace given a sensor reading.

3.0이 의미하는 것은 3.0m 이상의 값은 어떤 식으로든 고려하지 않을 것이라는 뜻이다. 위 두가지 값을 본인의 lidar 수준에 맞게 설정하면 되겠다.

footprint 차체의 xy평면상에서의 사이즈를 의미한다. 로봇이 지나가면서 로봇의 크기만큼 공간을 차지하는데, 이를 두고 footprint라고 한다. 무게중심을 0,0으로 생각하고 나머지 점들을 채워주면 되겠다. 보통은 사각형으로 설정하고 원한다면 robot_radius라고 하며 원형으로 설정하여도 되겠다.
inflation radius inflation radius는 navigation 중, lidar가 벽을 scanning 할 것이다. 로봇이 벽에 얼마나 붙어서 갈것인지 정하고 싶다면 수정해줘야하는 값이다. 0.55의 의미는 로봇이 obstacle로 부터 0.55m 떨어진 상태에서 path를 계획할 것이라는 뜻이다. 나는 좁은 곳에서 로봇을 움직여야 해서 값을 정말 많이 줄였다.
cost scaling factor 다음의 함수에 적용되는 요소이다. 음수와 곱해짐으로, 값이 커질 수록 cost value가 작아진다. 값을 작게 한다면 cost value가 커진다는 뜻. cost가 높아진다면 그쪽길로 가는 것을 피할 것이다.
```
exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1),
```

그림으로 본다면 다음과 같은 그림이 존재한다. 빨간색pcl이 lidar sensor값, 파란색이 inflation radius에 의한 그림이다. 빨간 box가 footprint이다.

만약 좁은곳에서 navigation을 실행하는데, inflation radius값을 크게 준다면 로봇은 움직이지 못할 것이다. 그러니 위의 파라미터들을 정말 세심하게 수정해주자.

5-3-2. global_costmap_params.yaml

global costmap의 configuration을 위한 파일이다.

global_costmap:
  global_frame: /map
  robot_base_frame: base_footprint
  update_frequency: 5.0
  publish_frequency: 5.0
  static_map: true
  transform_tolerance: 0.5

global_frame과 robot_base_frame은 costmap이 고려할 frame을 정의하는 부분이다. 그 다음 costmap의 update loop을 5.0 HZ로 설정한 모습이다. 이부분은 10.0으로 바꾸어서 작성했다. 동일하게 costmap을 publish하는 HZ이다. update와 publish는 다르다. static_map부분이 중요한데, map server로 부터 받은 맵을 초기화할 필요가 있는지 없는지 판단하는 것이다.

5-3-3. local_costmap_params.yaml

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint
  update_frequency: 10.0
  publish_frequency: 10.0
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05

이번에는 local costmap인데, update, publish는 최대한 동일한 값을 사용해주도록 하자. rolling_window는 다음과 같은 설명이 붙는다.

Setting the "rolling_window" parameter to true means that the costmap will remain centered around the robot as the robot moves through the world

간단히 해석 가능하다.

5-3-4. dwa_local_costmap_params.yaml

DWAPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.22
  min_vel_x: -0.22

  max_vel_y: 0.0
  min_vel_y: 0.0

# The velocity when robot is moving in a straight line
  max_vel_trans:  0.22
  min_vel_trans:  0.11

  max_vel_theta: 2.75
  min_vel_theta: 1.37

  acc_lim_x: 2.5
  acc_lim_y: 0.0
  acc_lim_theta: 3.2 

# Goal Tolerance Parameters
  xy_goal_tolerance: 0.05
  yaw_goal_tolerance: 0.17
  latch_xy_goal_tolerance: false

# Forward Simulation Parameters
  sim_time: 1.5
  vx_samples: 20
  vy_samples: 0
  vth_samples: 40
  controller_frequency: 10.0

# Trajectory Scoring Parameters
  path_distance_bias: 32.0
  goal_distance_bias: 20.0
  occdist_scale: 0.02
  forward_point_distance: 0.325
  stop_time_buffer: 0.2
  scaling_speed: 0.25
  max_scaling_factor: 0.2

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05

# Debugging
  publish_traj_pc : true
  publish_cost_grid_pc: true

DWA Path planning 알고리즘 configuration 파일이다. navigation 중 로봇의 속도/가속도를 파라미터로 줄 수 있다. 본인이 원하는 값으로 세팅해주면 되겠다.

중요한 파라미터라 생각하는 것은 ~ tolerance라고 생각한다. 우리는 로봇이 완벽히 정확하다고 생각하지 않는다. 그렇기에 로봇이 목표지점에 가까이 가도, 완벽하게 goal안에 들어갈 수 없다. 하지만 tolerance parameter를 통해 그 정도를 정할 수 있다. 값을 키울 수록 더 완벽하게 goal안에 들어가야 한다. 본인이 원하는 값을 세팅해주도록 하자.

5-4. chrony time synchronize

ROS1 Navigation을 사용하다 보면 시간 동기 에러가 나올때가 많다.

[ WARN] [1638649873.141034902]: Clearing both costmaps outside a square (3.00m) large centered on the robot.
[ WARN] [1638649878.240998667]: Rotate recovery behavior started.
[ERROR] [1638649879.541438854]: Rotate recovery can't rotate in place because there is a potential collision. Cost: -1.00
[ WARN] [1638649884.642048298]: Clearing both costmaps outside a square (1.84m) large centered on the robot.
[ WARN] [1638649889.742037155]: Rotate recovery behavior started.
[ERROR] [1638649889.742448519]: Rotate recovery can't rotate in place because there is a potential collision. Cost: -1.00
[ERROR] [1638649894.842041682]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors

특히 host pc와 master pc가 나뉜 상태에서 하려면 그렇다. 전의 한컴 아카데미에서는 다른 팀원들은 시간 동기화를 위해서 wifi 공유기를 장착한 걸로 기억한다(ntp방식). 그 후 인터넷 접속하여 시간을 동기화 하였다. 하지만 그 방법이 실제 자율주행을 해야하는 로봇이 시간 동기화를 위해 인터넷 접속을 한다는 것이 이해가 안되었다. 그래서 찾은 방법이 chrony를 이용한 시간동기화 방식이다.

5-4-1. jetson nano hotspot mode

먼저 젯슨 나노를 wifi hot spot모드로 만들어주자. 그렇게 한다면 ip주소가 10.42.0.1로 될 것이다. 물론 이같은 방법이 맘에 안든다면 본인이 원하는 방식으로 하면 된다. 특히 보안같은 것을 중요하게 여긴다면 다른 방식을 선택하자.

https://ubunlog.com/en/how-to-create-a-wifi-access-point-in-ubuntu-18-04-lts/ 위 링크에서 간단하게 실행하면 되겠다. 정말 간단하다.

5-4-2. install chrony in jetson nano(Master PC)

sudo apt install chrony
sudo vim /etc/chrony/chrony.conf

설정파일 바꾸기

pool <기준PC IP주소> iburst maxsources 1 # <기준PC IP주소> = 10.42.0.1

keyfile /etc/chrony/chrony.keys
driftfile /var/lib/chrony/chrony.drift

local stratum 10
allow 192.168.0.0/16

logdir /var/log/chrony
maxupdateskew 100.0
rtcsync
makestep 1 3

실행

sudo systemctl restart chronyd.service
sudo systemctl restart chrony.service
watch chronyc tracking # 실행대기

5-4-3. install chrony in labtop(Host PC)

chrony 설치 동일

설정파일 바꾸기.

$ sudo vi /etc/chrony/chrony.conf

pool <기준PC IP주소> iburst maxsources 1 # 10.42.0.1

keyfile /etc/chrony/chrony.keys
driftfile /var/lib/chrony/chrony.drift

logdir /var/log/chrony
maxupdateskew 100.0
rtcsync
makestep 1 3

sudo systemctl restart chrony.service

chrony 동작 확인

watch chronyc tracking

6. gmapping

http://wiki.ros.org/slam_gmapping/Tutorials/MappingFromLoggedData map을 만들기 위해선 다양한 방법이 있다. lidar를 사용하는 slam에서는 cartographer, gmapping 등등이 있고, camera를 이용하는 방법 또한 다양하다. 하지만 대회에 나간다는 생각이면 가장 간단한 방법을 사용하여 많은 테스트를 하는 것이 좋다고 생각한다.

6-1. setting

위에서 calibration을 했다면 필요한 세팅은 다 끝났다 생각한다. 그보다는 설치가 더 중요할 듯 하다.

6-2. 설치 및 사용법

sudo apt-get install ros-melodic-gmapping
rosmake gmapping
# 본인만의 launch file을 만들자. 관련 자료는 github에 있다.
roslaunch hanwhabot_slam hanwhabot_slam.launch
# map이 어느정도 완성이 되었다고 생각되면 아래의 명령어로 map을 저장한다.
rosrun map_server map_saver -f

이렇게 설치한 map을 navigation에서 사용하면 되겠다.

roslaunch hanwhabot_bringup hanwhabot_robot.launch
roslaunch hanwhabot_navigation hanwhabot_navigation_map.launch

이후 rviz 상에서 원하는 곳에 점을 찍으며 이동시키는 것을 확인하면 되겠다. rviz 세팅은 본인이 따로 해주도록 하자.

내가 추가적으로 목적지를 설정해 준 파일이 있다. mission5를 위한 것이며, 이는 거기서 직접 맵을 만든 후 initial point와 first goal, second goal을 한 스크립트 안에서 실행하기 위해 넣어준 파이썬 파일이다. hanwhabot_send_goal에서 확인할 수 있다.

rosrun hanwhabot_send_goal map5.py

핵심은 본인이 만든 맵에서 rviz 상으로 목적지의 좌표를 확인하는 것이다. 이를 알아낼 수 없다면 목표지점 자체를 모르는 것이니 이걸 할 이유가 없는 것!

8. Line Detection

line detection은 정말 많은 레퍼런스를 봤지만, 가장 중요한 것은 안의 내부 코드보다 카메라의 pose라고 생각한다. 우리는 이부분에 대해서 실험할 시간이 부족했고, 그로인해 내부 코드와 별개로 실패했다고 생각한다. 먼저 본인 차량의 속도와 주변 line의 길이를 고려해보자. 그리고 camera의 FoV를 고려한다면 camera의 높이가 몇이 나와야 하는지도 생각하면 얼추 그림이 나올 것이다. 나같은 경우에는 속도는 엄청 느린데, 카메라 시야에는 80cm 전방의 line만 보이고 높이 또한 낮아서 line이 제대로 보이지 않았다. 높이 약 30cm에 그런일이 있었다고 생각하면 꽤나 가슴아픈 일이다. 하지만 여기에는 코드만 설명하는 곳이므로 코드만 설명해보자. 참고자료이다.

다른 레퍼런스들도 많았지만, 대부분 위 두가지에서 같은 뿌리를 갖고 있었다. 후자부터 분석하면 허프라인디텍션을 이용하여 라인을 추출하고 있다. 전자는 이미지를 색으로 분류하여 라인을 추출하고, 두 라인에 대해 x = ay^2 + by + c 식을 만들어 a, b, c를 구하여 평균 라인을 구한다. 그 후 평균 라인의 한 픽셀과 이미지 중앙선 차이로 pid 계산을 하여 속도를 조절한다.

8-1. Feedback

위 방식의 안타까운 점 두가지가 있다. 첫번째는 굵은 선 부분이다. 굳이 따지자면, 2차식을 구할 필요가 없이 그냥 두 픽셀의 평균 값만 구해도 되는 부분이였다. 두번째로 안타까운 것은 정지선의 색 유무이다. turtlebot autorace를 살펴보니 정지선의 색이 빨강색으로 기억한다. 하지만 한화디펜스 대회는 정지선이 흰색이다. 이렇다보니 왼쪽과 오른쪽을 구분할 수 없게 된다. 그렇다고 후자의 코드 또한 단점은 있다. 필요한 라인만 선택하는 것이 아닌 좌우 그리고 정지선을 모두 추출한다는 것이다. 각각의 선에 의미를 부여하는 일 또한 필요하다는 것이다. 물론 이는 조금만 수정한다면 쉬울 듯 하다. 세번째로 안타까운 것은 계산량이다. 이것은 첫번째 부분과 어느정도 연관이 되는데, 2차식을 구하는 방식이 전체 히스토그램을 이용하는 것이 조금 안타까웠다. 개인적인 피드백으로는 RANSAC을 이용하여 피드백을 구하는 것은 어땠을까? 물론 계산량은 줄이겠지만 파라미터 세팅하는데에도 시간이 걸렸을 것이다.

Project Feed Back

전체적으로 아쉬운 프로젝트였다. 12월 중순 대회인데 10월 중순부터 차체를 만든거 치곤 잘했다곤 생각하나, 실제 코드를 테스트해보고 파라미터를 수정하는데 성공한건 미션 5번 뿐이라는 것이 안타까웠다.

전체 미션에 대한 피드백을 준다면 다음과 같다.

mission1: 카메라의 높이 조절을 통한 좀 더 수월한 line detection, 혹은 fov가 더 넓은 fisheye camera를 사용해볼것, RANSAC을 사용해서 계산량을 줄여볼 것.
mission2: 캐터필러의 탈선이 문제였다. 만일 탈선이 안되었다면 좀 더 수월 했을것.
mission3: 시뮬레이션을 통해 디자인을 수정했다면 만드는 시간이 좀 더 짧았을 것. 시뮬레이션을 통한 피드백이 아니라 실제 만들면서 피드백을 주다보니 시간이 배로 걸렸다.
mission4: 로봇 팔을 다룰줄 몰랐다는 점. 매니퓰레이터를 다룰줄 아는 팀원을 구해서 도전?!
mission5: 캐터필러의 탈선으로 인해 rotation calibration을 계속 해줘야 한다는 점이 아쉬웠다. 더 간단한 kinematics를 가진 로봇을 만든다면 이러한 걱정 없을 듯 하다.

결론 - 시뮬레이션과 매니퓰레이터를 할줄 아는 친구가 필요하다~!

https://github.com/cjh1995-ros/hanwhabot.git

2. VSLAM 테스트

Mon, 17 Jan 2022 16:11:50 GMT

VSLAM 테스트는 총 세가지 ORB_SLAM2, OpenVSLAM, RTABMAP을 진행하였다. 관련 영상 링크는 아래에 있다. https://youtu.be/WZDmh8JAfcU 위 같은 내용을 진행하기 위해 필요한 세팅에 대해 알아보자.

1. ORB_SLAM2, OpenVSLAM, RTABMAP install in jetson agx xavier

1-1. ORB_SLAM2 ros install

original ORB_SLAM2는 ros를 지원하긴 하지만 pcl이 pangolin viewer에서만 나오고 특별히 ros를 이용해서 할 수 있는 기능이 거의 없었던 걸로 기억한다. 또한 build시 따로 추가해야하는 헤더가 많아 귀찮았던걸로 기억한다. ros에 특화된 패키지는 https://github.com/appliedAI-Initiative/orb_slam_2_ros 이 링크의 패키지이며, 설치 방법 또한 쉽다.

1-2. OpenVSLAM install

https://openvslam-community.readthedocs.io/en/latest/installation.html 해당링크에서 install 하는 방법을 따라가면 된다. 참고로 opencv 4.1.1로도 실행가능하니 설치된 opencv를 삭제하지는 말자. 또한 Pangolin Viewer를 사용할지 말지를 기준으로 build하게 되는데, socket통신보다는 pangolin viewer가 훨씬 보기 편하다. 필자는 docker에서 설치하겠다고 거기있는 설치방법 그대로 가면 설치에 실패했다. 몇가지 설정을 바꾸어서 docker에서 설치를 성공했어도 결국 pangolin viewer가 제대로 작동하지 않아 base에 설치했다.

1-3. RTAB MAP install

https://github.com/introlab/rtabmap_ros 해당링크에서 install하는 방법을 따라가면 된다. nvidia jetson에서는 다음과 같은 것을 주의해서 설치하면 되겠다. https://github.com/introlab/rtabmap/issues/427#issuecomment-608052821

2. pkg에 필요한 내용.

3 패키지 모두 realsense, zed를 지원해서 두 카메라를 사용할 때는 특별히 추가할 것이 없다. 하지만 만일 본인만의 카메라를 사용한다고 하면 두가지 모두 camera tf를 추가해주자. tf를 추가하는 방법은 다음 차시에 설명할 것이다.

간단하게 launch 파일을 조작해보자.

2-1. ORB_SLAM2 customizing launch file

본인이 monocamera를 사용한다면 orb_slam2_mynteye_s_mono.launch 를 복사해서 이름을 바꾸고 본인것으로 사용해보자.

바꿔야 할 부분은 본인의 camera calibration 값을 넣고, camera/iamge_raw를 본인의 토픽으로 바꾸면 되겠다. 또한 scaleFactor를 본인 카메라에 맞게 넣어주면 되겠지만, 알다시피 monocamera의 scaleFactor는 알아내기 까다로우므로 일단 존재하는 것만 사용해도 무리가 없다.

만일 stereo 카메라라면 base line부분을 추가로 바꿔주면 되겠다.

ORB_SLAM2의 trajectroy, 즉 Path를 보려면 다음과 같은 것을 활용해야 한다. PoseStamped PoseStamped geometry_msgs에서 보자. http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html rviz상에서 보면 PoseStamped는 화살표 방향의 것으로만 알 수 있지, 시각적으로 보이는 것은 아니다. 그러므로 우리는 이것을 보기 위해 tf를 추가함과 동시에 nav_msgs path 쪽으로 만들어줄 필요가 있다. 하지만 trajectory를 시각적으로 보는 것이 아닌, 데이터만 저장하고 싶다면 이는 불필요한 내용이다.

Python: PoseStamped to Path

~~ 내용 추가 ~~

같은 이유로 OpenVSLAM도 동일한 과정을 겪는다. 하지만 개인적인 경험으로는 loopclosure를 경험하기 힘들었다. 두 슬램 모두에게서 말이다. 이는 loop closing이 얼마나 힘든 것인지 알 수 있다.

2-2. OpenVSLAM customizing launch file

https://openvslam-community.readthedocs.io/en/latest/ros_package.html#installation OpenVSLAM ros 패키지이다. 위 과정을 통해 설치하면 되지만, 특별한 launch 파일은 존재하지 않는다. 그래서 원한다면 추가로 만들어줘야 한다.

run_slam.launch

rosrun image_transport republish
raw in:=/usb_cam/image_raw raw out:=/camera/image_raw

run_localization.launch

위에서 config 파일이나 fbow의 경로는 기존 openvslam 패키지의 경로를 의미한다. 정 원한다면 ros 패키지 안에 따로 만들어 놓아도 되겠다.

config file

config.yaml 파일을 본인이 원하는 내용으로 작성하도록 하자. 나는 camera intrinsic을 제외하곤 바꾸지 않았다.

# EuRoC monocular model

#==============#
# Camera Model #
#==============#

Camera:
  name: "EuRoC monocular"
  setup: "monocular"
  model: "perspective"

  fx: 458.654
  fy: 457.296
  cx: 367.215
  cy: 248.375

  k1: -0.28340811
  k2: 0.07395907
  p1: 0.00019359
  p2: 1.76187114e-05
  k3: 0.0

  fps: 20.0
  cols: 752
  rows: 480

  color_order: "Gray"

#=====================#
# Tracking Parameters #
#=====================#

Tracking:
  max_num_keypoints: 1000
  ini_max_num_keypoints: 2000

#================#
# ORB Parameters #
#================#

Feature:
  scale_factor: 1.2
  num_levels: 8
  ini_fast_threshold: 20
  min_fast_threshold: 7

#====================#
# Mapping Parameters #
#====================#

Mapping:
  baseline_dist_thr_ratio: 0.02
  redundant_obs_ratio_thr: 0.9

#===========================#
# PangolinViewer Parameters #
#===========================#

PangolinViewer:
  keyframe_size: 0.07
  keyframe_line_width: 1
  graph_line_width: 1
  point_size: 2
  camera_size: 0.08
  camera_line_width: 3
  viewpoint_x: 0
  viewpoint_y: -0.65
  viewpoint_z: -1.9
  viewpoint_f: 400

stereo 또한 동일하게 바꿔주면 되겠다. OpenVSLAM은 rviz상에서 확인하기 위해 camera pose를 바꿔줘야 한다. camera_pose

Python: camera pose to path

~~ 내용 추가 ~~

2-3. RTAB MAP launchfile customizing??

https://doongdoongeee.tistory.com/105 http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping RTAB MAP의 경우 정말 잘 되있어서 따로 custom 하기 보단 위 링크에서 명령어를 복사하여 사용해보도록 하자. 정말 사용하는데 문제가 없다.

FeedBack

Pure Rotation for monocular

OpenVSLAM, ORB_SLAM2 monocular slam의 경우 모두 제자리 회전을 할 시 translation이 너무 작아서, 0에 근접해서 essential matrix를 구하는데 문제가 생겼었다(E=t^R). 그래서 본인의 trajectorty를 잃었다는 error가 계속해서 나오기도 했다. 하지만 stereo camera를 사용한 경우에는 그러한 문제가 없었다.

Velocity

너무 빠른 속도로 움직였을 때는 정면의 feature를 잃기도 했다. 물론 실험 당시에는 turtlebot 적정속도로 해서 문제는 없었지만 만일 카메라가 측면을 바라보며 빠른 속도로 달린다면 주변 feature를 쉽게 잃어버릴 것이다. 이는 적절한 속도를 생각하는 필요성도 있다는 것을 암시한다.

단순 비교

위 영상 링크에서 다른 영상들을 보면 3가지 slam을 비교하는 것을 볼 수 있다. 하지만 2d grid map을 만들 수 있던 것은 rtab map 뿐이었고, 나머지는 3d pcl을 추가로 처리해야 만들 수 있다는 것을 알았다. 하지만 아직 나에게는 너무나 어려운 일이였기에 성공하진 못했다. 또한 좁은 장소에서 해서 그런가 loop closing을 거의 못해냈다. 특히 ORB_SLAM2, OpenVSLAM의 경우 실내 공간을 한바퀴 돌았을 때 사각형을 그려야 하는데 그러지 못하는 모습을 보고 많이 실망했다. 물론 내가 한 실험방법이 틀린 걸 수 도 있다. 특히 sota를 받은걸 생각하면, 좀 더 공부하고 만져야 한다는 것이다. 아직은 dbow나 fbow를 파악하지 못했으므로 이것과 관련된 것만 더 공부한다면 앞으로 더욱 정진할 수 있을 것 같다.

실험시 힘든 점

ssh 로 pangolin viewer를 볼 수 없다는 점이 상당히 짜증났다. 그런걸 고려하면 socket통신으로 build하여 보는 것도 나쁘지 않다고 생각한다.

1. 저가 카메라에서 고가카메라 기능 구현

Mon, 17 Jan 2022 11:24:16 GMT

WaveShare사의 저가 카메라 IMX219-83 stereo 카메라 모듈을 이용하여 고가 카메라의 기능 중 depth estimation 기능을 구현하려 했다. 비록 성공적이진 못했지만, 이곳에서 제공하는 코드를 분석해보고, 차후에 더 개발해보자.

1. stereo camera depth estimation

스테레오 카메라가 depth를 추정하는 과정은 아래와 같다. 스테레오 카메라의 양안 모두 pinhole 모델이라 가정하고 풀어보자.

간단한 삼각측량법이다. 먼저 두 카메라 렌즈로 부터 이미지를 받아들이고 3D point P 가 양 렌즈에 PL과 PR로 projected 될 것이고, 우리는 삼각형의 닮음을 이용하여 다음과 같은 식을 얻어낼 수 있다. 위 식을 통해 얻어낸 것은 z 값이라는 것. 그런데 d는 무엇일까? disparity라고 하며, 같은 점을 두 카메라가 바라보았을 때 생기는 x pixel값 차이이다. 만약에 카메라가 left right로 정렬된 것이 아니라 수직상태로 정렬되었다면, y pixel 값 차이로 동일한 모델을 만들어 쓰면 되겠다.

어찌되었든 우리는 z값을 얻을 수 있고, 카메라로부터 얼마나 떨어져 있는지 계산할 수 있게 된다. 하지만 위와 같은 PL, PR을 우리는 얻을 수 있는가라고 묻는다면 답은 '네니오' 라고 생각한다. 이 매칭과정이 정말 어려워서 계속해서 딥러닝이나 다른 논문이 나오는 수준이다.

가장 먼저해야하는 것은 두 이미지의 epipolar plane을 맞추는 것이다. 우리가 갖는 두 이미지의 매칭이 되는 epipolar plane을 맞춘다는 것은, 같은 feature를 같은 높이에서 바라본다는 의미이고, 이를 해야지만 위와 같은 계산을 할 수 있다(꼭 필요하냐고 물어본다면 크게 차이가 나지 않는다면, depth map을 만들때만 사용하지, feature를 이용한 depth estimation에서는 그렇게 차이가 나지 않는다. 하지만 우리는 depth map을 만든다는 가정하에 얘기하는 것). 이 과정을 stereo rectification이라고 하고, 이미지상에는 다음과 같은 선으로 표현할 수 있다. 어떤 느낌인지 감이 오는가? 단순하게 같은 특징을 갖는 라인을 같은 높이에 맞춰놓는다라고 생각하면 편하다.

그렇다면 순서대로 생각해보자.

각 카메라를 calibration 하여 intrinsic, extrinsic matrix를 구한다.
rectification 과정을 통해 두 이미지의 epipolar line을 같은 높이선상에 둔다.
matching 과정을 통해 disparity를 구한다.
계산을 통해 depth map을 구한다.

이렇게 한다면 depth estimation은 끝이다. 그렇다면 실제 calibration 과정과 필요한 opencv 함수에 대해 알아보자.

2. Calibration

calibration은 총 세가지를 분석 했다. WaveShare, ROS, LearnOpenCV 핵심이 되는 곳만 짚어보자. 다운로드나 사용 링크는 아래에 남겨두겠다.

2-1. WaveShare

cameraMatrix[0] = initCameraMatrix2D(objectPoints,imagePoints[0],imageSize,0);
cameraMatrix[1] = initCameraMatrix2D(objectPoints,imagePoints[1],imageSize,0);
Mat R, T, E, F;

double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
                    cameraMatrix[0], distCoeffs[0],
                    cameraMatrix[1], distCoeffs[1],
                    imageSize, R, T, E, F,
                    CALIB_FIX_INTRINSIC,
                    TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) );

Mat R1, R2, P1, P2, Q;
Rect validRoi[2];

stereoRectify(cameraMatrix[0], distCoeffs[0],
                  cameraMatrix[1], distCoeffs[1],
                  imageSize, R, T, R1, R2, P1, P2, Q,
                  CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]);

    // Precompute maps for cv::remap()
initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);

remap(src, dst, rmap[0][0], rmap[0][1], INTER_LINEAR);
remap(src, dst, rmap[1][0], rmap[1][1], INTER_LINEAR);

calibration 과정 중 매번 나오는 것인 체스보드 calibration 과정을 기억할 것이다. 위 과정은 이미 체스보드의 교차점들을 objectPoints, imagePoints[2]에 포함되어 있다.

initCameraMatrix2D를 통해 우리는 intrinsic matrix를 얻을 수 있다.
stereoCalibrate을 통해 Rotation matrix, Translation vector, Essential Matrix, Fundamental Matrix를 구한다. 여기서 구하는 R, T는 왼쪽 카메라를 기준으로 볼때, 오른쪽 카메라의 rotation과 translation을 의미한다. 만일 본인이 반대 방향으로 했다면 반대방향으로 생각할 것.
stereoRectify을 통해 이미지를 정렬한다. 이때, R1, R2, P1, P2, Q, validRoi[2]가 존재하는데,

R1: 1번 카메라를 위한 rectification matrix(rotation matrix 3x3) 이다.
P1: 1번 카메라를 위한 Translation matrix
Q: 4x4의 disparity to depth mapping matrix이다. 이를 이용하여 point cloud를 만들 수 있다. 자세히는 reprojectTo3D 함수를 보자.
validRoi: 사용 가능한 rectification 이미지 부분이다. alpha값에 의해 달라진다(1) validRoi란 위와 같이 초록색 부위를 의미한다.

그런데, 위에서 보통 1(왼쪽)을 기준으로 생각하기 때문에 P1, P2 쌍은 다음과 같이 생기는게 보통이다. 우리는 rectification 과정을 통해 cy1, cy2 값을 갖게 한것을 잊지 말자.

initUndistortRectifyMap 함수를 통해 remap에서 사용할 undistortion하고 rectification transform할 수 있는 matrix를 얻자. 두가지 rmap을 요구하는데, 각각 x, y축에 관한 내용이다.
마지막으로 remap함수를 통해 원하는 이미지를 만들자.

2-2. LearnOpenCV

 cv::Mat mtxL,distL,R_L,T_L;
 cv::Mat mtxR,distR,R_R,T_R;

 cv::Mat new_mtxL, new_mtxR;

 // Calibrating left camera
 cv::calibrateCamera(objpoints,
                      imgpointsL,
                      grayL.size(),
                      mtxL,
                      distL,
                      R_L,
                      T_L);

 new_mtxL = cv::getOptimalNewCameraMatrix(mtxL,
                                distL,
                                grayL.size(),
                                1,
                                grayL.size(),
                                0);

 // Calibrating right camera
 cv::calibrateCamera(objpoints,
                      imgpointsR,
                      grayR.size(),
                      mtxR,
                      distR,
                      R_R,
                      T_R);

 new_mtxR = cv::getOptimalNewCameraMatrix(mtxR,
                                distR,
                                grayR.size(),
                                1,
                                grayR.size(),
                                0);

LearnOpenCV에서는 다른 방식으로 intrinsic matrix를 구했다. 이외에는 동일한 방식으로 진행한다.

2-3. ROS1

위의 코드를 본인이 이해하고 싶지 않다거나, 보고 싶지 않고 단순히 구현된 것으로 intrinsic matrix, extrinsic matrix, rotation, translation, essential, fundamental 을 구하고 싶다면 ROS의 구현되어 있는 UI를 이용하면 좋겠다. 방법은 간단하다. 아래의 shell을 따라가자.

# http://wiki.ros.org/camera_calibration/Tutorials/StereoCalibration
# 의존성 확인
rosdep install camera_calibration
rosmake camera_calibration
# 카메라 켈리브레이션 approximate 옵션은 카메라 timestamp delay를 0.1초 정도 허용해준다. 아래에서 자신의 topic에 맞는 이름을 넣어주자.
rosrun camera_calibration cameracalibrator.py --approximate 0.1 --size 8x6 --square 0.108 right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw right_camera:=/my_stereo/right left_camera:=/my_stereo/left

이후에는 약 40장 정도의 사진을 찍게 되겠다. 그 후 calibrate를 한 뒤 save해주면 되겠다.

3. get depth map

위에서까지는 rectification이 된 map을 얻었다면, 이제는 disparity를 구해야 한다. disparity를 구하려면 두 이미지에서 동일한 점을 계산할 수 있어야 하는데, 이에 대해서 정말 다양한 방법이 있다. StereoBM, StereoSGBM, 딥러닝 등등이 있는데, 간단하게 opencv에서 제공하는 StereoSGBM 사용법만 파악하고 넘어가자.

Ptr sgbm = StereoSGBM::create(0,64,11);
/*
parameter setting
*/

Mat disp(left.size(), CV_16S), depth;

sgbm->compute(left, right, disp);

위에서 파라미터 세팅이 있는데 잠시만 넘어가고, 어쨋든 sgbm->compute를 하면 disparity를 계산해준다는 것이다. 하지만 그냥 넘어 갈 수 없는 것이 있다. 현재 만들어진 disparity의 데이터 형이 CV_16S라 imshow()를 해서 보일 수 없다는 것이다.

잘 생각해보면 disparity를 이용해서 depth를 구하는데, depth에 음수가 있을 수 있나?(disparity는 음수가 있을 수 있다) 또한 OpenCV에서 표현할 수 있는 depth의 한계는 CV_16U(cm)이다. 약 65m가 한계라는 뜻이다. 어쨋든 우리는 disparity를 한번 normalizing한 뒤 depth를 표현할 필요가 있다는 것이다.

normalizing 하는 방식에서 WaveShare사와 LearnOpenCV의 차이가 있다.

아래는 waveshare사이다.

double minVal; double maxVal;
minMaxLoc( disp, &minVal, &maxVal );
add(disp, -minVal, disp);
divide(disp, 16, disp);
disp.convertTo( disp, CV_8UC1);

signed 16bit integer 인것을 알고, minMaxLoc을 통해 disp의 최소값, 최대값을 구한 뒤, add를 통해 disp의 최소값을 0으로 맞춰놓았다. 그 후 16으로 나눈다. 그 후에 8UC1로 바꾸는다.

다음은 LearnOpenCV이다.

disp.convertTo(disparity,CV_32F, 1.0);
disparity = (disparity/(float)16.0 - (float)minDisparity)/((float)numDisparities);

여기서 numDisparities는 다음과 같다. numDisparities – Maximum disparity minus minimum disparity. This parameter must be divisible by 16. 16배수의 수이며, max disparity - minimum 값이라고 한다. 고로 normalizing 할 수 있는 값이다. 하지만 OpenCV에서는 다음과 같은 함수를 제공한다.

void cv::normalize(InputArray src,
                   InputOutputArray dst,
                   double alpha = 1,
                   double beta = 0,
                   int norm_type = NORM_L2,
                   int dtype = -1,
                   InputArray mask = noArray() 
)
// 다음과 같은  normalize가 가능하다.
normalize(disp, disp8, 0, 255, CV_MINMAX, CV_8U);

위 같은 경우는 그냥 imshow() 하기 위한 것일까? 개인적으로는 disp8이 아닌, disp16으로 하고 CV_16U를 사용하여 65m 거리를 표현하는 것이 더 좋아보인다. 이 함수에 대해서는 더 알아볼 필요가 있지만, 단순히 imshow()를 하기 위해서는 인터넷에서 자주 사용하는 것으로 보인다.

WaveShare사의 normalizing 방식은 개인적으로 정석적이라 생각한다. 16s데이터 형식을 32f로 바꾸어주며 normalizing 해주는 것. 그 후 depth를 계산한 뒤 ushort형식(unsigned 16bit int)로 바꾸어 depth를 표현해 주는 것. 그리고 그 이미지를 imshow 해주는 것. imshow는 ushort를 표현해 줄 수 있다고 한다. 물론 내용을 바꾸어 표현하지만, 실제 값을 바꾸는 것은 아니니 상관없다고 생각한다.

LearnOpenCV 방식은 imshow를 고려하지 않았다고 생각한다. 하지만 depth를 구하기에는 무리가 없다.

두 식들로 depth를 구해보자.

z = fb/d 임을 기억하는가? 각각의 disparity를 fb에 나누고, 그 값을 넣어준 matrix를 만들면 되겠다.

다음은 WaveShare사의 코드이다.

void disp2Depth(cv::Mat dispMap, cv::Mat &depthMap, double Disp2Depth)
{
    int type = dispMap.type();

    if (type == CV_8U)
    {
        int height = dispMap.rows;
        int width = dispMap.cols;

        uchar* dispData = (uchar*)dispMap.data;
        ushort* depthData = (ushort*)depthMap.data;
        for (int i = 0; i < height; i++)
        {
            for (int j = 0; j < width; j++)
            {
                int id = i*width + j;
                if (!dispData[id]) {
                    depthData[id] = 65535;
                    continue;
                }  
                depthData[id] = ushort( (float)Disp2Depth/ ((float)dispData[id]) );
            }
        }
    }
    else
    {
        cout << "please confirm dispImg's type!" << endl;
        cv::waitKey(0);
    }
}

여기서 Disp2Depth값은 fb인데, 이를 다음과 같이 표현했다.

Disp2Depth = Q.at(2, 3) * -T.at(0, 0);

Q는 아래와 같다. T는 translation vector인데, 1->2 값이니까 baseline이다. 하지만 방향이 반대이므로 -값을 취한것으로 보인다. ~~개인적인 생각으론 그냥 parameter로 만들어서 계산하는게 맞다고 본다. 굳이 복잡한 방식의 표현보다는.~~ 어찌 되었든 값은 fb라는 것이다. 고로 depth 값은 맞다.

4. stereo matching parameter

OpenCV의 StereoMatching 모듈은 정말 작성은 쉽지만, 원하는 깔끔한 맵을 보기에는 세밀한 파라미터 튜닝이 필요한 듯 하다. 필자도 적당한 param 선정방식을 아직 정하지 못했다. 하지만 각각의 파라미터에 대한 이해는 필요하다. 그러므로 아래의 글을 보자.

numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width/8) + 15) & -16;
sgbm->setPreFilterCap(63);
int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
sgbm->setBlockSize(sgbmWinSize);
int cn = img1.channels();   
sgbm->setP1(8*cn*sgbmWinSize*sgbmWinSize);
sgbm->setP2(32*cn*sgbmWinSize*sgbmWinSize);
sgbm->setMinDisparity(0);
sgbm->setNumDisparities(numberOfDisparities);
sgbm->setUniquenessRatio(10);
sgbm->setSpeckleWindowSize(100);
sgbm->setSpeckleRange(32);
sgbm->setDisp12MaxDiff(1);
sgbm->setMode(StereoSGBM::MODE_SGBM);

minDisparity - possible disparity 최소값. 보통은 0인데, rectification 알고리즘이 이미지를 이동시켜서 제대로 선정해야함.
numDisparities - Max - min disparity. 16으로 나뉘어져야함.
blockSize - Matched block size. 1이상의 홀수여야함. 보통 3~11사이에서 선정됨.
P1 - The first parameter controlling the disparity smoothness.
P2 - 동일한 기능. 그 값이 클 수록 disparity가 부드럽다. 항상 P2>P1이여야 한다. 좋은 P1, P2는 다음과 같이 선정된다. 8number_of_image_channelsblockSizeblockSize and 32number_of_image_channelsblockSizeblockSize
disp12MaxDiff - Maximum allowed difference (in integer pixel units) in the left-right disparity check. 음수로 체크하면 이 기능을 사용하지 않는다. 이유는 모르겠지만 대부분 +1로 선정한다.
preFilterCap - Truncation value for the prefiltered image pixels, 필터링된 이미지를 자르는 값 크기.
uniquenessRatio - 보통 5~15사이로 선정된다.
SpeckleWindowSize - 보통 50~200 사이로 선정된다.
SpeckleRange - 16으로 곱해져야하며, 보통 1,2 가 선정된다.
Mode - StereoSGBM::MODE_HH로 하면 run the full-scale two-pass dynamic programming algorithm 된다.

speckle - 작은 반점. windowsize에 대한 영상 이미지이다. 보고 참고하면 좋을 듯 하다.

Learning OpenCV 3에서 파라미터 세팅에대한 자세한 얘기가 나와있다. 만약 본인이 흥미가 있다면 "Stereo Correspondence" 부분을 찾아서 읽어보자. page는 pdf 파일 기준 737~759(19장)이다.

5. Easy Start With ROS

위의 일련의 과정들이 어느정도 이해가 되었다고 하자. 하지만 이를 직접 작성하는 것은 무척 힘든 일이다. 물론 관련하여 참고 링크를 올려놓았지만 만약 본인은 이러한 과정보다는 그냥 결과만 바란다면 ROS에서 제공하는 패키지들을 이용하면 편하겠다. 물론 ROS 사용자들에게만 편하다... https://answers.ros.org/question/317961/rtabmap-how-to-view-or-export-the-disparity-images-from-stereo-sgm/ 정말 답변이 잘 나와있는 링크이다. sgbm말고 bm방식으로도 사용할 수 있다. 일단 위 링크에서 calibration을 하려면, rosbagfile 사용하는 것이 좋다. 그러한 파일이 있다면 다음과 같은 방식으로 시작하면 되겠다.

ROS_NAMESPACE=narrow_stereo_textured rosrun stereo_image_proc stereo_image_proc

rosrun rqt_reconfigure rqt_reconfigure

calibration은 위같은 방식으로 하고, 아래의 링크로 원하는 pcl을 얻도록 하자! 정말 잘 정리된 깃헙이며, IMX219-83 ros라고 검색하면 바로 나오는 github이다! https://github.com/borongyuan/jetson_csi_stereo_ros 추가로 이 링크에서 jetson nano에 ROS1을 OpenCV4.1.1 로 사용할 수 있는 Vision Pkg도 있으니 어렵게 melodic 버전 맞춘다고 downgrade하지 말고 저걸 사용해보자!

[ref]

stereorectify 함수: https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga617b1685d4059c6040827800e72ad2b6
WaveShare사 코드: https://www.waveshare.com/wiki/IMX219-83_Stereo_Camera
LearnOpenCV 깃헙 링크: https://github.com/spmallick/learnopencv/blob/master/stereo-camera/calibrate.cpp
LearnOpenCV Stereo Camera Calibration 방법: https://learnopencv.com/making-a-low-cost-stereo-camera-using-opencv/
Stereo Calibration with ROS1: http://wiki.ros.org/camera_calibration/Tutorials/StereoCalibration
https://docs.opencv.org/3.4/d1/d1b/group__core__hal__interface.html#ga4a3def5d72b74bed31f5f8ab7676099c
StereoSGBM 파라미터 세팅: https://docs.opencv.org/4.x/d2/d85/classcv_1_1StereoSGBM.html
Stereo Basic: https://www.youtube.com/watch?v=hUVyDabn1Mg
stereo camera parameter setting with ROS: https://answers.ros.org/question/317961/rtabmap-how-to-view-or-export-the-disparity-images-from-stereo-sgm/
stereo camera parameter setting with ROS: http://wiki.ros.org/stereo_image_proc/
IMX219-83 ROS: https://github.com/borongyuan/jetson_csi_stereo_ros

md

Mon, 27 Dec 2021 03:14:31 GMT

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "md/vel_msg.h"
#include "md/monitor_msg.h"

//mskim
#include 
#include 
#include 
#include 
#include 

#include 
#define DEG2RAD(x) (x * 0.01745329252) // *PI/180
#define RAD2DEG(x) (x * 57.2957795131) // *180/PI

typedef unsigned char  BYTE;

#define LEFT                 0
#define RIGHT             1

#define BIT0              0x01
#define BIT1              0x02
#define BIT2              0x04
#define BIT3              0x08
#define BIT4              0x10
#define BIT5              0x20
#define BIT6              0x40
#define BIT7              0x80



//mskim
float odom_pose[3];
float odom_vel[3];
double global_yaw;
bool is_nomotionupdate;

typedef struct {
    long lPosiX;
    long lPosiY, lExPosiY;
    short sTheta;
    float fTheta;
    short sVoltIn;
    short sCurrent[2];
    BYTE byUS1;
    BYTE byUS2;
    BYTE byUS3;
    BYTE byUS4;
    BYTE byPlatStatus;
    BYTE byDocStatus;
    BYTE byMotStatus[2];
    BYTE byRun;
    BYTE fgAlarm[2], fgCtrlFail[2], fgOverVolt[2], fgOverTemp[2];
    BYTE fgOverLoad[2], fgHallFail[2], fgInvVel[2], fgStall[2];

    BYTE fgEmerON, fgBatChargeON, fgRccState;

    short sCmdLinearVel, sCmdAngularVel;
    short sRealLinearVel, sRealAngularVel;

    float fRealLinearVel, fRealAngularVel;

    int nAngleResolution;

}MotorDriver;
MotorDriver Md;

//It is a message callback function.
//It is a function that oprates when a topic message named 'monitor_topic' is received.
//The input message is to receive the 'monitor_msg' message from 'md' package in msg directory
void monitorCallBack(const md::monitor_msg::ConstPtr& monitor)
{
    int nGap;
    static int nExsTheta;

    Md.lPosiX             = monitor->lPosiX;
    Md.lPosiY             = monitor->lPosiY;
    Md.sTheta             = monitor->sTheta;
    Md.fTheta             = (float)(monitor->sTheta)/10;
    Md.sRealLinearVel     = monitor->sRealLinearVel;
    Md.fRealAngularVel    = (float)(monitor->sRealAngularVel)/10;
    Md.sVoltIn            = monitor->sVoltIn;
    Md.sCurrent[LEFT]     = monitor->sLeftMotCur;
    Md.sCurrent[RIGHT]    = monitor->sRightMotCur;
    Md.byUS1              = monitor->byUS1;
    Md.byUS2              = monitor->byUS2;
    Md.byUS3              = monitor->byUS3;
    Md.byUS4              = monitor->byUS4;
    Md.byPlatStatus       = monitor->byPlatStatus;
    Md.byDocStatus        = monitor->byDocStatus;
    Md.byMotStatus[LEFT]  = monitor->byLeftMotStatus;
    Md.byMotStatus[RIGHT] = monitor->byRightMotStatus;
    Md.byRun              = monitor->byRun;

    Md.fgEmerON      = Md.byPlatStatus & BIT0;
    Md.fgBatChargeON = Md.byDocStatus & BIT0;
    Md.fgRccState    = (Md.byDocStatus & BIT7) >> 7;

    Md.fgAlarm[LEFT]    = Md.byMotStatus[LEFT] & BIT0;
    Md.fgCtrlFail[LEFT] = Md.byMotStatus[LEFT] & BIT1;
    Md.fgOverVolt[LEFT] = Md.byMotStatus[LEFT] & BIT2;
    Md.fgOverTemp[LEFT] = Md.byMotStatus[LEFT] & BIT3;
    Md.fgOverLoad[LEFT] = Md.byMotStatus[LEFT] & BIT4;
    Md.fgHallFail[LEFT] = Md.byMotStatus[LEFT] & BIT5;
    Md.fgInvVel[LEFT]   = Md.byMotStatus[LEFT] & BIT6;
    Md.fgStall[LEFT]    = Md.byMotStatus[LEFT] & BIT7;

    Md.fgAlarm[RIGHT]    = Md.byMotStatus[RIGHT] & BIT0;
    Md.fgCtrlFail[RIGHT] = Md.byMotStatus[RIGHT] & BIT1;
    Md.fgOverVolt[RIGHT] = Md.byMotStatus[RIGHT] & BIT2;
    Md.fgOverTemp[RIGHT] = Md.byMotStatus[RIGHT] & BIT3;
    Md.fgOverLoad[RIGHT] = Md.byMotStatus[RIGHT] & BIT4;
    Md.fgHallFail[RIGHT] = Md.byMotStatus[RIGHT] & BIT5;
    Md.fgInvVel[RIGHT]   = Md.byMotStatus[RIGHT] & BIT6;
    Md.fgStall[RIGHT]    = Md.byMotStatus[RIGHT] & BIT7;

    //nGap = Md.sTheta - nExsTheta;
    //ROS_INFO("%4d %4d", nGap, Md.sTheta);
    //nExsTheta = Md.sTheta;
    //ROS_INFO("%d, %d, %d, %d", Md.sCmdLinearVel, Md.sRealLinearVel, Md.sCmdAngularVel, Md.sRealAngularVel);
    /*printf("sub-> x: %ld  y: %ld  theta: %3.1f  linearVel: %d  angularVel: %4.1f  L_Cur: %d  R_Cur: %d  US1:%d  US2:%d  US3:%d  "
           "volt:%d  Emergecy:%d  Charge:%d  DocState:%d  LeftMotStatu:%d  RightMotStatu:%d\n",
           Md.lPosiX, Md.lPosiY, Md.fTheta, Md.sRealLinearVel, Md.fRealAngularVel, Md.sCurrent[LEFT], Md.sCurrent[RIGHT], Md.byUS1, Md.byUS2, Md.byUS3, Md.sVoltIn,
           Md.fgEmerON, Md.fgBatChargeON, Md.fgRccState, Md.byMotStatus[LEFT], Md.byMotStatus[RIGHT]);*/
    //printf("encoder x: %ld  y: %ld  theta: %3.1f\n", Md.lPosiX, Md.lPosiY, Md.fTheta);
    //printf("L_Cur: %d  R_Cur: %d Cur: %d\n", Md.sCurrent[LEFT], Md.sCurrent[RIGHT]+Md.sCurrent[LEFT], Md.sCurrent[RIGHT]);
    if(Md.fgAlarm[RIGHT] != 0){
      printf("RIGHT\n");
      printf("ALARM = %d\n", Md.fgAlarm[RIGHT]);
      printf("Ctrl Fail= %d\n", Md.fgCtrlFail[RIGHT]);
      printf("Over Volt = %d\n", Md.fgOverVolt[RIGHT]);
      printf("Over Temp = %d\n", Md.fgOverTemp[RIGHT]);
      printf("Over Load = %d\n", Md.fgOverLoad[RIGHT]);
      printf("Hall Fail = %d\n", Md.fgHallFail[RIGHT]);
      printf("Inv Vel = %d\n", Md.fgInvVel[RIGHT]);
      printf("Stall = %d\n", Md.fgStall[RIGHT]);
      printf("Current = %d\n", Md.sCurrent[RIGHT]);
      // 21.01.15 sykim add - auto motor fault reset
      system("rostopic pub /motor_reset_topic std_msgs/Empty -1 &");
    }

    if(Md.fgAlarm[LEFT] != 0){
      printf("LEFT\n");
      printf("ALARM = %d\n", Md.fgAlarm[LEFT]);
      printf("Ctrl Fail= %d\n", Md.fgCtrlFail[LEFT]);
      printf("Over Volt = %d\n", Md.fgOverVolt[LEFT]);
      printf("Over Temp = %d\n", Md.fgOverTemp[LEFT]);
      printf("Over Load = %d\n", Md.fgOverLoad[LEFT]);
      printf("Hall Fail = %d\n", Md.fgHallFail[LEFT]);
      printf("Inv Vel = %d\n", Md.fgInvVel[LEFT]);
      printf("Stall = %d\n", Md.fgStall[LEFT]);
      printf("Current = %d\n", Md.sCurrent[LEFT]);
      // 21.01.15 sykim add - auto motor fault reset
      system("rostopic pub /motor_reset_topic std_msgs/Empty -1 &");
    }

    //mskim, odometry pose set
    //odom_pose[0] = Md.lPosiY*0.001;
    //odom_pose[1] = 0;
    //odom_pose[2] = DEG2RAD(Md.fRealAngularVel);
    //printf("Md.lPosiY [0] = %f,\t Md.lPosiX [1] = %f,\t RAD = %f\n", odom_pose[0], odom_pose[1], odom_pose[2]);
    //printf("sRealAngularVel = %f\n", odom_pose[2]);

}

/////////////////////////////////////for example to get the 'Md.sCmdAngularVel'////////////////
void keyboardCallBack(const geometry_msgs::Twist& keyVel)   //from turtlebot3_teleop_key node
{
    Md.sCmdLinearVel  = keyVel.linear.x * 1000;// mm/s
    if(Md.nAngleResolution)
        Md.sCmdAngularVel = keyVel.angular.z * 10;// 0.1 deg/s
    else
        Md.sCmdAngularVel = keyVel.angular.z;// 1 deg/s
}
///////////////////////////////////////////////////////////////////////////////////////////////


//mskim
void cmdVelCallBack(const geometry_msgs::Twist& cmdVel)   //from turtlebot3_teleop_key node
{
    //keyboard or navigation value
    odom_vel[0] = cmdVel.linear.x;
    odom_vel[1] = cmdVel.linear.y;
    odom_vel[2] = cmdVel.angular.z;

    Md.sCmdLinearVel  = cmdVel.linear.x * 1000;// mm/s
    if(Md.nAngleResolution)
        Md.sCmdAngularVel = RAD2DEG(cmdVel.angular.z) * 10;// 0.1 deg/s
    else
        Md.sCmdAngularVel = RAD2DEG(cmdVel.angular.z);// 1 deg/s

    //printf("\ncmdVel[0] = %f,\t cmdVel[1] = %f,\t cmdVel[2] = %f\t\n", odom_vel[0], odom_vel[1], odom_vel[2]);    
}


void imuCallBack(const sensor_msgs::Imu& imu_vel)   //from turtlebot3_teleop_key node
{
    double roll=0;
    double pitch=0;
    double yaw=0;

    tf::Quaternion q(imu_vel.orientation.x, imu_vel.orientation.y, imu_vel.orientation.z, imu_vel.orientation.w);
    tf::Matrix3x3 m(q);
    m.getRPY(roll, pitch, yaw);

    global_yaw = yaw;
    //printf("sub-> yaw: %3.1f\n", global_yaw);
}

void nomotionCallBcak(const std_msgs::Empty::ConstPtr & _is_nomotionupdate)
{
    is_nomotionupdate = true;
}


//Node main function
int main(int argc, char** argv)
{
    static int nResolution, nCnt1, nOperMode, nResetOdometry, nResetAngle;

    ros::init(argc, argv, "vel_cmd_node");                                                //Node name initialization.
    ros::NodeHandle nh;                                                                   //Node handle declaration for communication with ROS system.
    ros::Publisher cmd_vel_pub = nh.advertise("vel_topic", 10);             //Publisher declaration.
    ros::Subscriber monitor_sub = nh.subscribe("monitor_topic", 10, monitorCallBack);    //Subscriber declaration.
    //ros::Subscriber keyboard_sub = nh.subscribe("cmd_vel", 10, keyboardCallBack); //original
    ros::Subscriber imu_sub = nh.subscribe("imu", 10, imuCallBack);    //mskim add
    ros::Subscriber keyboard_sub = nh.subscribe("cmd_vel", 10, cmdVelCallBack); //mskim add
    ros::Subscriber nomotion_sub = nh.subscribe("need_nomotionupdate", 10, nomotionCallBcak);

    //mskim add, for call rosservice
    ros::ServiceClient nomotionUpdateClient = nh.serviceClient("/request_nomotion_update");
    std_srvs::Empty srv;
    //mskim, add odometery publisher, tf publisher
    ros::Publisher odom_pub = nh.advertise("odom", 50);

    ros::Publisher is_move_pub = nh.advertise("is_move", 50);

    tf::TransformBroadcaster tf_broadcaster;

    //original code
    ros::Rate r(20);                                                                      //Set the loop period -> 50ms.

    nOperMode = 0;
    md::vel_msg vel;          //vel_msg declares message 'vel' as message file.
    nh.getParam("md_node/angleresolution", Md.nAngleResolution);
    /////////////////////////////////////////////////////////////////

    //mskim add
    std_srvs::Empty::Request req;
    std_srvs::Empty::Response resp;
    std_msgs::Int32 move_data;
    // Odometry Initialize
    nav_msgs::Odometry odom;
    odom.pose.pose.position.x = 0.0;
    odom.pose.pose.position.y = 0.0;
    odom.pose.pose.position.z = 0.0;

    odom.pose.pose.orientation.x = 0.0;
    odom.pose.pose.orientation.y = 0.0;
    odom.pose.pose.orientation.z = 0.0;
    odom.pose.pose.orientation.w = 0.0;

    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    for (int index = 0; index < 3; index++)
    {
      odom_pose[index] = 0.0;
      odom_vel[index] = 0.0;
    }

    double x = 0.0;
    double y = 0.0;
    double th = 0.0; 
    int is_move_count = 0;
    int nomotionupdate_count = 0;

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();



    while(ros::ok())
    {
        if(nCnt1++ == 10)     //Store the value of the parameter in the variable once per 500mS.
        {
            nCnt1 = 0;
            nh.getParam("vel_cmd_node/reset_odometry", nResetOdometry);
            nh.getParam("vel_cmd_node/reset_angle", nResetAngle);
        }
        if(Md.byRun)
        {
            move_data.data = 1;
        }
        else{
            move_data.data = 0;
        }
        if(is_move_count++ == 20)
        {
            is_move_count = 0;
            is_move_pub.publish(move_data);
        }
        if(is_nomotionupdate)
        {
            if(nomotionupdate_count<1000){
                nomotionUpdateClient.call(srv);
                nomotionupdate_count++;
        }
            else
            {
                is_nomotionupdate = false;
                nomotionupdate_count = 0;
            }
        }
        vel.nLinear         = Md.sCmdLinearVel;
        vel.nAngular        = Md.sCmdAngularVel;
        vel.byResetOdometry = nResetOdometry;     //Put the value of 'nResetOdometry' in the child byResetOdometry message of 'vel'
        vel.byResetAngle    = nResetAngle;
        cmd_vel_pub.publish(vel);                 //Publish the message 'vel'

        ros::spinOnce();
    current_time = ros::Time::now();

        double dt = (current_time - last_time).toSec();
        double delta_x = (odom_vel[0] * cos(th) - odom_vel[1] * sin(th)) * dt;
        double delta_y = (odom_vel[0] * sin(th) + odom_vel[1] * cos(th)) * dt;
        double delta_th = odom_pose[2] * dt; //odom_vel[2] * dt;

        x = (Md.lPosiX/1000.0)*(-1);
        y = (Md.lPosiY/1000.0)*(-1);
        th = Md.fTheta-90;
        //x += delta_x;
        //y += delta_y;
        //th += delta_th;
    //ROS_INFO("x:%f y:%f th:%f\n",x ,y ,th);
        //ROS_INFO("dt:%f\n",dt);
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(DEG2RAD(th));
        // temp blocked, mskim, test odom_combined
/*        geometry_msgs::TransformStamped odom_tf;
        odom_tf.header.stamp = current_time;
        odom_tf.header.frame_id = "odom";
        //odom_tf.child_frame_id = "base_link";
       //odom_tf.child_frame_id = "base_footprint";
        odom_tf.transform.translation.x = x;
        odom_tf.transform.translation.y = y;
        odom_tf.transform.translation.z = 0.0;
        odom_tf.transform.rotation = odom_quat;

        //send the transform
        tf_broadcaster.sendTransform(odom_tf);
*/  
        //next, we'll publish the odometry message over ROS
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";

        //set the position
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;

        //set the velocity
        //odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = (Md.sRealLinearVel)/1000.0;
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.angular.z = DEG2RAD(Md.fRealAngularVel);

    //jmlee add covariance 210607
    //odom.pose.covariance[0] = 0.01;
    //odom.pose.covariance[7] = 0.01;
    //odom.pose.covariance[14] = 0.01;
    //odom.pose.covariance[21] = 0.1;
    //odom.pose.covariance[28] = 0.1;
    //odom.pose.covariance[35] = 0.1;
    //odom.twist.covariance = odom.pose.covariance;
    //sykim add covariance
     odom.pose.covariance[0] = 0.0001;
     odom.pose.covariance[7] = 0.0001;
     odom.pose.covariance[14] = 10000000000.0;
     odom.pose.covariance[21] = 10000000000.0;
     odom.pose.covariance[28] = 10000000000.0;
     odom.pose.covariance[35] = 0.001;
     odom.twist.covariance = odom.pose.covariance;
        /*
    //sykim add covariance
    odom.pose.covariance = [0.000001, 0, 0, 0, 0, 0,
                0, 0.000001, 0, 0, 0, 0,
                0, 0, 0.000001, 0, 0, 0,
                0, 0, 0, 0.000001, 0, 0,
                0, 0, 0, 0, 0.000001, 0,
                0, 0, 0, 0, 0, 0.000001]
    odom.twist.covariance = [99999, 0, 0, 0, 0, 0,
                 0, 99999, 0, 0, 0, 0,
                 0, 0, 99999, 0, 0, 0,
                 0, 0, 0, 99999, 0, 0,
                 0, 0, 0, 0, 99999, 0,
                 0, 0, 0, 0, 0, 99999]
*/
    //printf("\ntrans.x = %f\t, trans.y = %f\t,\n", odom_tf.transform.translation.x,odom_tf.transform.translation.y);
        //printf("linear.x = %f\t, angular.z = %f\n\n", Md.sRealLinearVel, Md.fRealAngularVel);
    //printf("linear.x = %f\t, linear.y = %f\t, angular.z = %f\n\n", odom.twist.twist.linear.x, odom.twist.twist.linear.y, odom.twist.twist.angular.z);

        odom_pub.publish(odom);
        last_time = current_time;

        r.sleep();
    }
}

ROS cpp class example

Sun, 26 Dec 2021 15:18:34 GMT

class_example_v1.h

#include 
#include 
#include 
#include 
#include 

class example
{
public:
  example(ros::NodeHandle* nh);
private:
  ros::NodeHandle nh_;
  ros::Subscriber sub_;
  ros::ServiceServer server_;
  ros::Publisher pub_;

  double val_from_sub_;
  double val_to_remember_;
  void initSub();
  void initPub();
  void initService();
  void subCB(const std_msgs::Float32::ConstPtr& msg);
  void serviceCB(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
};

class_example_v1.cpp

#include "example.h"
example::example(ros::NodeHandle* nh):nh_(nh)
{
  initSub();
  initPub();
  initService();
  val_to_remember_ = 0.0;
}

void example::initSub()
{
  sub_ = nh_.subscribe("exSubTopic", 1, example::subCB, this);
}

void example::initPub()
{
  pub_ = nh_.advertise("exPubTopic", 1, true);
}

void example::initService()
{
  server_ = nh.advertiseService("exService", &example::serviceCB, this);
}

void example::subCB(const std_msgs::Float32 msg)
{
  val_from_sub_ = msg.data;
  std_msgs::Float32 output;
  val_to_remember_ += val_to_sub_;
  output.data = val_to_remember_;
  pub_.publish(output);
}

bool example::serviceCB(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
{
  res.success = true;
  return true;
}

cpp_class_v2.cpp

#include 
#include 
#include 

class NumberCounter {

    private:
    int counter;
    ros::Publisher pub;
    ros::Subscriber number_subscriber;
    ros::ServiceServer reset_service;

    public:
    NumberCounter(ros::NodeHandle *nh) {
        counter = 0;

        pub = nh->advertise("/number_count", 10);    
        number_subscriber = nh->subscribe("/number", 1000, 
            &NumberCounter::callback_number, this);
        reset_service = nh->advertiseService("/reset_counter", 
            &NumberCounter::callback_reset_counter, this);
    }

    void callback_number(const std_msgs::Int64& msg) {
        counter += msg.data;
        std_msgs::Int64 new_msg;
        new_msg.data = counter;
        pub.publish(new_msg);
    }

    bool callback_reset_counter(std_srvs::SetBool::Request &req, 
                                std_srvs::SetBool::Response &res)
    {
        if (req.data) {
            counter = 0;
            res.success = true;
            res.message = "Counter has been successfully reset";
        }
        else {
            res.success = false;
            res.message = "Counter has not been reset";
        }

        return true;
    }
};

int main (int argc, char **argv)
{
    ros::init(argc, argv, "number_counter");
    ros::NodeHandle nh;
    NumberCounter nc = NumberCounter(&nh);
    ros::spin();
}

[ref]

ros arduino test

Tue, 21 Dec 2021 13:40:30 GMT

#!/usr/bin/env python
from re import T
import rospy
from std_srvs.srv import *
from sensor_msgs.msg import LaserScan
import numpy as np

class testArduinoService(object):
    def __init__(self):
        self.test_client = rospy.ServiceProxy("test_srv2", Trigger)
        self.trig_req = TriggerRequest()
        self.trig_res = TriggerResponse()
        self.subs_lidar = rospy.Subscriber("scan", LaserScan, self.get_scan_data)
        self.scan = None

    def get_scan_data(self, scan):
        self.scan = np.array(scan)

if __name__=="__main__":
    rospy.init_node("get_service")
    testArduinoService().do_req()

cjh1995-ros.log

NVIDIA Omniverse Basic Environment setup

목차

Setup Global Stage Properties

Setup Global Physics Properties

Add Ground Plane

Add Lighting

NVIDIA Omniverse basic interfaces

기본 Viewport 조작

기본 물체 제어

참고자료

Install NVIDIA SDK Manager in Ubuntu 20.04

Reference

설치해도 안됐을 때

되는거 고려한 링크

Install NVIDIA VPI for Elbrus SLAM

1. What is VPI?

Install Error

Solution

2. Install Isaac Packages in ROS2_ws

Nvidia Omniverse (Isaac SIM)

Task 이해하기

1. Task Class 이해하기

2. World Class에서 task 활용하기.

Nvidia Omniverse (Isaac SDK + SIM)

치명적 단점

Nvidia Omniverse (Isaac SDK)

Isaac samples

setup

Installing Dependencies on Desktop

기본 사용법

하나만 빌드

전체 테스트

하나만 테스트

sample 사용

Run Jupyter notebook

SLAM and DL Paper Lists

Lidar SLAM

VSLAM

Object Detection

Depth estimation

Dataset

Awesome

NVIDIA Omniverse 시작 - 00

Nucleus

Connect

Kit

Simulation

RTX Renderer

Nvidia Create

ROS1, ROS2, Conda 같이 쓰기

1. ros1 설치하기

2. ros2 설치하기

3. conda 설치하기

4. 셋다 alias로 실행하기

ROS2 RVIZ Marker

ROS2 Gazebo

1. WareHouse 제작

설치방법

2. Turtlebot3 불러오기

3. RVIZ에서 보기

4. Intel Realsense Gazebo Plugin 사용

urdf vs sdf

Required

Optional

4. d435i Plugin 포기.

포기 이유.

기본 plugin 활용

5. Google Cartographer

6. RTAB-Map

7. Navigation

Feed back

ROS2 realsense

ROS2 Realsense Wrapper Install

Realsense d435i calibration

Realsense d435i launch

ROS URDF to SDF

URDF란?

SDF란?

어떻게 바꾸나?