민 [simulator] Lidar Clustering Algorithm

3. Lidar Clustering Algorithm

Lidar point를 의미있는 결과로 묶는 것. 가장 간단한 방법은 거리가 가까운 점들을 하나의 segment로 묶는 방법이다.

 

시뮬레이터를 다시 연결해주자.

kw-cobot@kwcobot-HGDT-Series:~$ roslaunch scout_sim_launch simulater.launch 

를 치고

시뮬레이터를 열어주자

 

2022-03-03 14:08:58+0900 [-] [INFO] [1646284138.824286]: Client connected.  1 clients total.
2022-03-03 14:08:58+0900 [-] [INFO] [1646284138.825934]: Client connected.  2 clients total.
2022-03-03 14:08:58+0900 [-] [INFO] [1646284138.829486]: Client connected.  3 clients total.
2022-03-03 14:08:58+0900 [-] [INFO] [1646284138.831409]: Client connected.  4 clients total.
2022-03-03 14:08:59+0900 [-] [INFO] [1646284139.952691]: Client connected.  5 clients total.
2022-03-03 14:08:59+0900 [-] [INFO] [1646284139.956212]: Client connected.  6 clients total.
2022-03-03 14:08:59+0900 [-] [INFO] [1646284139.960260]: Client connected.  7 clients total.
2022-03-03 14:09:00+0900 [-] [INFO] [1646284140.823624]: [Client 1] Subscribed to /scout_light_control
2022-03-03 14:09:00+0900 [-] [INFO] [1646284140.831037]: [Client 2] Subscribed to /cmd_vel

총 7개의 client가 자동으로 연결됨을 볼 수 있다.

 

 

kw-cobot@kwcobot-HGDT-Series:~$ rostopic echo /lidar2D
header: 
  seq: 1
  stamp: 
    secs: 1646284206
    nsecs: 305000000
  frame_id: "velodyne"
angle_min: -3.14159274101
angle_max: 3.14159274101
angle_increment: 0.0174532923847
time_increment: 0.000125000005937
scan_time: 0.0450000017881
range_min: 0.0
range_max: 10.0
ranges: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 

 

 

kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src/scout_sim_launch/scripts$ touch lidar_cluster.py
kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src/scout_sim_launch/scripts$ chmod +x lidar_cluster.py 
kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src/scout_sim_launch/scripts$ ls
imu_sub.py  lidar_cluster.py  publisher.py
kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src/scout_sim_launch/scripts$ 

 

 

#!/usr/bin/env python
#-*- coding:utf-8 -*-

기본으로 넣어주자.

 

 

kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show obstacle_detector/Obstacles 
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
obstacle_detector/SegmentObstacle[] segments
  geometry_msgs/Point first_point
    float64 x
    float64 y
    float64 z
  geometry_msgs/Point last_point
    float64 x
    float64 y
    float64 z
obstacle_detector/CircleObstacle[] circles
  geometry_msgs/Point center
    float64 x
    float64 y
    float64 z
  geometry_msgs/Vector3 velocity
    float64 x
    float64 y
    float64 z
  float64 radius
  float64 true_radius

~kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show obst
acle_detector/SegmentObstacle 
geometry_msgs/Point first_point
  float64 x
  float64 y
  float64 z
geometry_msgs/Point last_point
  float64 x
  float64 y
  float64 z

 

 

결과

총 0-359번째 laser scan 값이 나오고

작은 거리에 있는 값들을 같은물체로 인식하여 번호를 묶는다.

[0, 23] 를

kw-cobot@kwcobot-HGDT-Series:~$ rosrun scout_sim_launch lidar_cluster.py 
[INFO] [1646288406.854937]: Initialized
[[0, 23], [24, 24], [25, 25], [26, 38], [39, 97], [98, 152], [153, 196], [197, 197], [198, 198], [199, 199], [200, 200], [201, 338], [339, 339], [340, 340], [341, 341], [342]]
[[0, 23], [24, 24], [25, 25], [26, 38], [39, 97], [98, 152], [153, 196], [197, 197], [198, 198], [199, 199], [200, 200], [201, 338], [339, 339], [340, 340], [341, 341], [342]]
[[0, 23], [24, 24], [25, 25], [26, 38], [39, 97], [98, 152], [153, 196], [197, 197], [198, 198], [199, 199], [200, 200], [201, 338], [339, 339], [340, 340], [341, 341], [342]]
[[0, 23], [24, 24], [25, 25], [26, 38], [39, 97], [98, 152], [153, 196], [197, 197], [198, 198], [199, 199], [200, 200], [201, 338], [339, 339], [340, 340], [341, 341], [342]]

 

실물에서 공부하는 방법

obstacle_detector 검색

resource 논문

 

 

위 그림처럼 cluster의 시작점과  끝점을 직선으로 나타내준다. (노란색 선)

오른쪽 아래 터미널 값은 Segment값을 출력한 값으로 번호로 군집을 묶은 값이 나오게 된다.

 

 

위 rviz 화면처럼 2D lidar 센서 중심으로 비슷한 거리끼리 센서값을 군집으로 묶기 때문에 로

실질적으로 멀리 떨어진 값이 노란색 선으로 연결되는 모습을 보인다.

 

장애물을 여러 개 두고 코드를 돌려본 결과 위 rviz 값처럼 노란색의 군집이 생성된다.

 

 

출력한 군집 값