turtlebot_navigation.launch파일에서 amcl.launch파일을 실행시키는데
이 amcl.launch파일은 ros/opt/kinetic 에 위치하는 amcl 패키지를 통해 실행된다.
(이는 sudo apt-get install ros-kinetic-amcl 을 통해 설치된 것으로
sudo apt-get purge ros-kinetic-amcl 을 통해 삭제가 가능하다)
그럼 이제 catkin_ws를 생성하고 여기에 turtlebot_navigation 패키지와 amcl패키지(ros-planning/navigation)을
설치해보자.
설치가 다 되었으면 해당 workspace를 빌드해보자.
(catkin_make)
우선 amcl 패키지의 src내에 존재하는 amcl_node.cpp 파일에 대한 내용을 분석해보자.
코드의 초반은 위와 같이 코드에서 사용될 변수들의 선언에 대한 내용들을 나타내고 있다.
그리고 AmlNode의 생성자 부분을 살펴보면 NodeHandler(private_nh_)에 대한 파라미터 초기화 내용이 진행된다.
nodehandler.param("AA", BB, CC);
위의 문장은 "노드의 파라미터 "AA"에 대하여 변수 BB의 값을 할당해주며 초기값은 CC로 한다"의 내용을 담고있다.
파라미터들에 대한 초기화가 마무리되면 바로 updatePoseFromServer() 함수가 실행된다.(line 441)
해당 함수는 1차원배열 init_pos_ 배열과 init_cov_ 배열의 원소에 값을 할당해주는 과정으로 보인다.
그럼 가장 윗부분만 살펴보자.
우선 init_pose_ 배열과 init_cov_ 배열을 초기화해준다.
그리고 파라미터 initial_pose_x 의 값을 tmp_pos값으로 초기화하는데 이때 값을 init_pose_[0]의 값으로 설정한다.
init_pose_[0]은 위에서 0.0의 값으로 초기화를 했으니
initial_pose_x 의 파라미터는 0.0으로 초기화되는 것으로 볼 수 있다.
그 후 std::isnan(tmp_pos) 가 등장하는데 이때 tmp_pos에는 0.0의 값이 들어가있고 isnan(0.0)은 False를 반환하게 될 것이다.
isnan은 괄호안의 값이 Nan(Not a number)인지를 확인하며 괄호안의 값이 숫자일경우 False, 숫자가 아닐경우 True를 반환한다.
따라서 if(!std::isnan(tmp_pos)) 는 정상적인 경우 if(!False) 이므로 해당되는 조건문으로 볼 수 있다.
그렇다면 tmp_pos는 init_pose_[0]의 값으로 초기화되고 다시 init_pose_[0]은 tmp_pos로 초기화된다.
(무슨 목적인지는 잘 이해되지 않는다.)
결과적으로 updatePoseFromServer() 함수는 init_pose_와 init_cov_에 대한 초기화를 진행하게 된다.
다음으로 ros의 duration에 1.0s를 할당해주는 부분, TF정보를 Broadcast, listen해주는 동적 객체를 생성한다.
그리고 amcl_pose, particlecloud 토픽에 대한 정보를 나타낼 수 있도록 pose_pub, particlecloud_pub_를 설정한다.
amcl_pose는 로봇의 실시간 position과 orientation을 나타내고(x, y, yaw) particlecloud는 particle들의 정보를 나타내는 것으로 보인다.
즉, particlecloud가 어떻게 수렴되어가는지를 확인해보면 될 것이다.
생성자의 나머지 부분들 역시 publish할 메시지들과 subscribe할 메시지들을 설정해주는 부분으로 볼 수 있다.
다음으로 등장하는 부분은 reconfigureCB 함수에 대한 내용이다.
자세한 내용을 살펴봐야 하겠지만 내부에 ROS_ERROR를 넣어 실행시켜본 결과
이 부분은 초기에 1회만 진행되고 중간에 위치인식을 수행하는데에는 실행되지 않는 것으로 보인다.
frame_id를 설정해주고 lidar센서의 셋팅들을 불러오는 것으로 확인된다.
다음으로는 rosbag에 대한 부분이다.
이는 저장된 bag파일을 통해 작업을 진행하기 위한 부분으로 보인다.
따라서 스킵하도록 하겠다.
다음으로는 savePoseToServer( ) 함수이다.
여기선 map_pose에 대하여 initial_pose_x와 initial_pose_y, 그리고 회전을 나타내는 initial_pose_a의 값을 설정한다.
이 부분은 노드가 실행될때 주기적으로 실행되는 부분이고 x, y값은 바퀴 엔코더의 회전량을 통해, yaw의 경우 imu의 신호를 통해 측정되는 것으로 보인다.
(로봇을 손으로 들어 이동시키면 yaw값이 변화하고 teleop을 통해 모터를 회전하면 x, y값이 변화한다.)
즉 이 부분이 로봇의 x, y, yaw의 값을 설정해주는 부분으로 보인다.
다음은 checkLaserReceived( )함수로 라이다센서의 측정 신호가 지정한 interval 내에 수신되는지 확인하기 위한 부분이다.
이 뒤에 연달아 등장하게 되는
AmclNode::requestMap() 함수와
AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg) 함수 역시 이미 생성된 global map을 불러오기 위한 과정으로 볼 수 있다.
다음은 handleMapMessage( ) 부분이다.
이 부분에 대한 역할을 이해하기 위해 함수 내 모든 코드를 주석처리 해보니 아래와같이 Laser의 신호도 제대로 받지 못하고
로봇의 모델을 표시하는데에도 문제가 발생하게 된다.
아마 지도정보를 제대로 받아오지 못하기에 발생한 것으로 보인다.
그리고 이 함수는 프로세스를 진행하면서 초기에 1회만 실행되는 것을 확인하였다.
그런데 중간에 내용을 살펴보면 다음과 같은 부분이 존재한다.
주석으로 "Create the particle filter" 로 되어있는데
이 부분을 통해 particle filter에 대한 설정을 진행하는 것으로 보인다.
이 부분을 좀 더 정확하게 분석하여 보자.
init_pose_배열에는 위에서 초기화된 값들이 x, y, yaw순서로 들어 있는데
이를 pf_init_pose_mean.v벡터에 넣는 것을 볼 수 있다.
즉 위치를 지정해주고 노이즈로 인한 불확실성을 추가한 후
pf_init( ) 함수를 통해 초기화를 진행하는 것으로 보인다.
pf_init() 함수는 /navigation/amcl/src/amcl/pf/ 경로에 있는 pf.c 파일에서 확인할 수 있다.
가우시안 분포를 통해 particle들을 초기화하는 것으로 보인다.
다음으로 주의깊게 볼 부분은 getOdomPose( ) 함수의 부분이다.
이 부분은 프로세스가 실행될 때 반복적으로 실행되는 부분으로
현재 로봇의 Odometry를 나타내는 부분으로 보인다.
함수의 자료형이 Bool 형태인걸로 봐서 odom정보를 잘 받아왔는지 아닌지를 판단하는 부분으로 보인다.
다음으로 등장하는 부분은 uniformPoseGenerator( ) 함수이다.
허나 이 함수의 내부에 ROS_ERROR()를 통해 실행시켜본 결과
출력되지 않는 것으로 보아 이는 특별하지 않으면 실행되지 않는 부분으로 봐도 될 것 같다.
다음은 laserReceived( ) 함수이다.
이 부분은 굉장히 코드의 길이가 길기도 하며 센서의 신호를 처리하는 중요한부분으로 보인다.
