RGB-D 카메라를 통해 Octomap생성까지 완료되었고 이를 통해 모바일로봇이 주행가능한 Gridmap생성까지 완료되었다.
이제 모바일로봇이 해당 GridMap을 통해 위치인식을 진행하고 경로를 계획하여 주행하는 단계를 진행하면 된다.
본 게시물에서는 우선 GridMap을 통해 위치인식을 진행하기 위한 과정을 다룬다.
-----------------------------------
우선 최종적으로 Gridmap은 경로를 계획하고 위치인식을 진행하기 위해 사용된다.
그렇기에 위치인식을 진행하는 AMCL 패키지에 이를 위한 노드를 생성하려 한다.
위와 같이 src 경로에 cpp파일을 생성하고 아래와 같이 CMakeLists.txt를 수정한다.
amcl_rgbd_node.cpp 파일은 RGB-D 카메라를 통해 정보를 받아와 지도에서 위치인식을 수행할 수 있도록 하는 노드를 생성하게 된다.
이를 위해 2.5차원의 지도를 불러와 Image Processing을 진행할 수 있어야 하고 이를 위해 별도의 노드를 만들어 작업을 진행해 보았다. 이 노드를 안정화한 후 위에서 생성한 amcl_rgbd_node.cpp 파일에 내용을 추가하는 방식으로 작업을 진행할 계획이다.
GridMap의 Message를 받아와 작업을 진행하기 위해 work space에 catkin_ws_map-processing을 만들어 OpenCV 라이브러리를 ROS에서 작업할 수 있는 환경을 갖추었다.
Workspace 경로에서
$ catkin_create_pkg 명령어를 통해 패키지를 생성하고
CMakelists는 OpenCV를 처리할 수 있도록 다음 내용들을 추가해줬다.
그럼 이제 본격적으로 메시지를 받아와 이를 처리하는 노드를 만들어보자.
위와 같이 Octomap을 실행한 후 GridMap을 통해 2.5차원 지도를 생성하게 되고 2.5차원 지도에 대한 메시지는
아래 결과에서 볼 수 있듯히 /grid_map_visualization/elevation_grid 의 토픽으로 전달되고 있음을 확인할 수 있다.
그럼 Subscriber는 해당 메시지를 받아와 처리하면 될 것이다.
먼저 /grid_map_visualization/elevation_grid 메시지의 타입을 확인해야 한다.
해당 메시지의 타입은 nav_msgs/OccupancyGrid 이다.
그럼 Subscriber에서 해당 타입의 메시지를 불러올 수 있도록 설정하는데 이를 위해 opt/ros/kinetic/include 에 해당 라이브러리가 있는지 확인한다.
아래 그림에서 확인할 수 있듯이 nav_msgs/OccupancyGrid.h가 존재한다.
nav_msgs/OccupancyGrid 의 메시지 구성은 위의 우측에 나타낸 것과 같으며
따라서 나는 Octomap을 통해 작성한 2.5D map(Elevation map)이 잘 받아오는지 확인하기 위해
아래와 같이 코드를 구성했다.
#include<cv_bridge/cv_bridge.h>
#include<ros/ros.h>
#include<iostream>
#include<opencv2/opencv.hpp>
//#include<opencv2/highgui/highgui.hpp>
#include "nav_msgs/GetMap.h"
#include "image_transport/image_transport.h"
#include "grid_map_msgs/GridMap.h"
#include "nav_msgs/OccupancyGrid.h"
#include "std_msgs/Float32MultiArray.h"
cv::Mat input_map;
using namespace cv;
void mapConvert(const nav_msgs::OccupancyGrid& msg){
try{
std::cout<<"msg.MetaData: "<<msg.info<<std::endl;
std::cout<<"msg.MetaData.width: "<<msg.info.width<<std::endl;
std::cout<<"msg.MetaData.height: "<<msg.info.height<<std::endl;
std::cout<<"msg.header: "<<msg.header<<std::endl;
input_map = cv::Mat(msg.info.height, msg.info.width, CV_8UC1);
for(int i=0; i<msg.info.height; i++){
for(int j=0; j<msg.info.width; j++){
int count = i*msg.info.width + j;
if(msg.data[count] <= 0)
input_map.at<uchar>(i, j) = 255;
else{
//std::cout<<" valid data: "<<int(msg.data[count]);
input_map.at<uchar>(i, j) = msg.data[count];
}
}
}
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert from");
}
}
int main(int argc, char** argv){
ros::init(argc, argv, "opencv_version_test");
ros::NodeHandle nh;
std::cout<<"OpenCV Version: "<<CV_VERSION<<std::endl;
image_transport::ImageTransport it(nh);
image_transport::Publisher pub = it.advertise("AA/image", 1);
//sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", input_map).toImageMsg();
ros::Subscriber sub = nh.subscribe("/grid_map_visualization/elevation_grid", 1000, mapConvert);
//ros::spin();
cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
ros::Rate loop_rate(5);
while(nh.ok()){
ros::spinOnce();
cv::equalizeHist(input_map, input_map);
if(input_map.cols != 0 && input_map.rows!=0){
imshow("view", input_map);
waitKey(5);
}
cv_ptr->encoding = "mono8";
cv_ptr->header.stamp = ros::Time::now();
cv_ptr->header.frame_id = "/AAAID";
cv_ptr->image = input_map;
pub.publish(cv_ptr->toImageMsg());
// sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", input_map).toImageMsg();
// pub.publish(img_msg);
loop_rate.sleep();
}
return 0;
}코드의 16~40 line은 콜백함수를 처리하는 부분으로 line52를 통해 지정한 Subscriber를 통해 작업을 수행하는 부분이다.
즉 line58의 while문을 통해 subscribe하기로 지정한 /grid_map_visualization/elevation_grid 메시지를 전달받게 되면 line16~40의 부분이 실행되고 결과적으로 전역변수로 선언된 Mat::input_map 변수를 정의하게 된다.
이 과정까지가 line59의 spinOnce()를 통해 이루어지는 과정이고 이후 전달받은 input_map을 통해 이를 imshow()로 시각화해주며 cv_ptr을 통해 RVIZ에서 확인할 수 있는 이미지 메시지로 변환하여 publish하게 된다.
위의 사진은 RVIZ에서 취득되는 2.5차원 지도를 Image 형태로 다시 Publish하는 결과를 나타낸다.