ROS Gazebo 강좌10
강좌9에서 생성한 Coverage map을 이용하여 로봇이 cover한 영역의 비율을 계산한다. Floorplan의 영역을 알 수 없기 때문에, 영상처리 방식으로 Coverage 비율을 계산한다. 따라서, 실시간으로 비율을 계산하지 않고, coverage 후에 floorplan과 coverage map을 포함하는 이미지를 캡쳐하여 별도의 ROS 노드를 만들어서 계산하는 방식을 택한다. 영상처리를 위해 OpenCV를 사용한다. 노드의 주요 부분은 영상처리 부분으로써, ROS가 필요하진 않지만 파라미터 사용 등의 편의를 위해 ROS의 노드 형태로 코딩한다. 참고로, 강좌9에서 캡쳐한 이미지를 mobile_robot_sim/output/coverage/input.png에 넣는다. 이 경로 및 이름은 파라미터로 launch파일에서 변경가능하다.
소스 파일은 mobile_robot_sim/src/coverage_proc.cpp로 정하였다. 소스 파일의 제일 첫 부분은 관련 헤더 파일을 인클루드 하는 것이다. 먼저 ROS 패키지 경로를 얻기 위한 <ros/package.h> 인클루드 한다. 그다음 OpenCV의 이미지 처리를 위한 헤더 파일을 인클루드 한다.
//////////// ros related
// ros
#include <ros/ros.h>
// ros package to access package directory
#include <ros/package.h>
// opencv
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
여기서도 파라미터를 별도의 struct를 만들어서 다른 변수와 구별한다. 파라미터로는 input 이미지 이름과 장애물 영역을 계산하기 위한 RGB color threshold 값들이 있다.
// parameter struct
struct CoverageParam
{
// input image filename
std::string input_filename;
// low and high blue values for obstacle areas
int obs_lb, obs_hb;
// low and high green values for obstacle areas
int obs_lg, obs_hg;
// low and high red values for obstacle areas
int obs_lr, obs_hr;
};
클래스 이름은 CoverageProc으로 한다. ROS의 역할은 파라미터를 읽는 정도이지만, 제대로 동작하기 위해서는 반드시 ros::NodeHandle 멤버를 선언하고, 초기화 해야 한다. NodeHandle을 선언하지 않으면 프로그램이 제대로 동작하지 않는다.
/*
* Coverage Calculation Process class
*/
class CoverageProc
{
public:
///
CoverageProc();
///
~CoverageProc();
///
void init();
///
void run();
///
void calculateCoverage();
private:
/// ros node handle
ros::NodeHandle ros_nh_;
///
CoverageParam params_;
/// input image file name including the path
std::string input_filename_;
};
별다른 역할은 없다.
/*
* Constructor
*/
CoverageProc::CoverageProc()
{
}
/*
* Destructor
*/
CoverageProc::~CoverageProc()
{
}
파라미터를 읽어서 구조체 변수를 초기화 한다. 주요 파라미터 변수는 장애물 영역을 계산하는데 필요한 RGB low, high threshold 값이다. 이것은 input 이미지에서 장애물 pixels의 color값을 분석하여 얻는다. 이 때, 우분투에서는 gpick과 같은 툴을 사용할 수 있다.
/*
* Initialize parameters
*/
void CoverageProc::init()
{
// private node handle for getting parameters
ros::NodeHandle ros_nh("~");
// input image filename
ros_nh.param<std::string>( "input_image", params_.input_filename, "coverage.png" );
//// parameters for thresholds to find obstacles in the input image
// low blue value
ros_nh.param<int>( "obs_lb", params_.obs_lb, 30 );
// high blue value
ros_nh.param<int>( "obs_hb", params_.obs_hb, 100 );
// low green value
ros_nh.param<int>( "obs_lg", params_.obs_lg, 200 );
// high green value
ros_nh.param<int>( "obs_hg", params_.obs_hg, 255 );
// low red value
ros_nh.param<int>( "obs_lr", params_.obs_lr, 200 );
// high red value
ros_nh.param<int>( "obs_hr", params_.obs_hr, 255 );
//// build the image filename including the package path
// get the package path
std::string pkg_path= ros::package::getPath( "mobile_robot_sim" );
// add the strings
input_filename_= pkg_path + "/" + params_.input_filename;
#if __DEBUG__
ROS_WARN( "input filename: %s", input_filename_.c_str() );
#endif
}
여기서는 단순히 coverage ratio를 계산하는 함수를 호출한다.
/*
* Run coverage calculation.
*/
void CoverageProc::run()
{
calculateCoverage();
}
Coverage Ration 계산 방법은 다음과 같다.
- input 이미지를 color로 연다.
- input 이미지에서 장애물 영역을 찾는다. 이 때, OpenCV의 inRange 함수를 사용한다.
- 장애물 영역만 나타난 이미지에서 pixel count하여 장애물 영역을 계산한다.
- 장애물 영역 이미지에서 contours 찾기를 하여 가장 큰 면적을 갖는 contour를 선택하고, 그 면적을 전체 면적으로 한다.
- input 이미지에서 coverage areas를 inRange 함수로 찾는다. coverage areas의 경우는 검은색으로 고정되어 있으므로 별도의 파라미터를 설정하지 않았다.
- coverage areas의 pixel count 하여 covered 영역을 계산한다.
- 전체 면적에서 장애물 뺀 면적 대비 covered 영역의 비율을 계산한다. 참고로, 내부가 비어있는 장애물이 있다면 장애물 영역 계산에 오차가 발생한다.
/*
* Calculate the coverage ratio.
*/
void CoverageProc::calculateCoverage()
{
// open the image file. use color image
cv::Mat input_img= cv::imread( input_filename_, CV_LOAD_IMAGE_COLOR );
// sanity check
if( !input_img.data ) // Check for invalid input
{
ROS_ERROR( "Could not open or the input image" );
return;
}
// obstacle image
cv::Mat obs_img;
// @note use an app such as gpick to know the pixel value
cv::inRange( input_img, cv::Scalar(params_.obs_lb, params_.obs_lg, params_.obs_lr), cv::Scalar(params_.obs_hb, params_.obs_hg, params_.obs_hr), obs_img );
// count the number of obstacle pixels
int obs_area= 0;
for( int y=0; y<obs_img.rows; y++ )
{
for( int x=0; x<obs_img.cols; x++ )
{
// count the obstacle pixels
if( obs_img.at<uchar>(y,x) == 255 )
{
obs_area++;
}
}
}
ROS_INFO( "obstacle area: %d", obs_area );
#if __DEBUG__
cv::namedWindow( "obs_img" );
cv::imshow( "obs_img", obs_img );
#endif
//// find contours of obstacle image to get the floorplan area
cv::vector<cv::vector<cv::Point> > contours;
cv::vector<cv::Vec4i> hierarchy;
cv::findContours( obs_img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
// random number generator for drawing contours
static cv::RNG rng(123);
// mat for drawing contours
cv::Mat drawing = cv::Mat::zeros( obs_img.size(), CV_8UC3 );
// find the largest contour and compute its area
int max_idx= 0;
double max_area= 0;
for( int i=0; i<contours.size(); i++ )
{
// update the max area and its index
if( cv::contourArea( contours[i] ) > max_area )
{
max_idx= i;
max_area= cv::contourArea( contours[i] );
}
cv::Scalar color= cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
cv::drawContours( drawing, contours, i, color, 2, 8, hierarchy, 0, cv::Point() );
}
ROS_INFO( "max area: %.2f", max_area );
//// obtain the covered area
cv::Mat covered_img;
// extract the covered grids from the input image
cv::inRange( input_img, cv::Scalar(0,0,0), cv::Scalar(30,30,30), covered_img );
// count the covered pixels
int covered_area= 0;
for( int y=0; y<covered_img.rows; y++ )
{
for( int x=0; x<covered_img.cols; x++ )
{
// increase the count
if( covered_img.at<uchar>(y,x) == 255 )
{
covered_area++;
}
}
}
ROS_INFO( "covered area: %d", covered_area );
// compute the coverage ratio
double coverage= ( covered_area*100.0/(max_area-obs_area) );
ROS_WARN( "coverage ratio: %.2f [%%]", coverage );
#if __DEBUG__
// input image
cv::namedWindow( "input_img" );
cv::imshow( "input_img", input_img );
// contour image
cv::namedWindow( "Contours", CV_WINDOW_AUTOSIZE );
cv::imshow( "Contours", drawing );
// coverage image
cv::namedWindow( "covered_img");
cv::imshow( "covered_img", covered_img );
cv::waitKey(0);
#endif
}
위에서 선언한 클래스의 객체를 만들고, init함수 호출 후, run함수를 호출한다.
/*
* main function
*/
int main( int argc, char **argv )
{
// ros init
ros::init( argc, argv, "coverage_proc_node" );
// create a coverage process instance
CoverageProc coverage;
// initialize coverage process
coverage.init();
// ros run
coverage.run();
// spin ROS
try
{
// ros takes all
ros::spin();
}
catch( std::runtime_error& e )
{
ROS_ERROR( "ros spin failed: %s", e.what() );
return -1;
}
return 0;
}
input 이미지 파일 이름과 장애물 영역 RGB thresholds를 파라미터로 정의하여 사용한다. 파일 이름은 launch/coverage_proc.launch
<!--
Coverage ratio calculation
-->
<launch>
<node pkg="mobile_robot_sim" type="coverage_proc_node" name="coverage_proc_node" output="screen" >
<param name="input_image" value="output/coverage/input.png" />
<param name="obs_lb" value="30" />
<param name="obs_hb" value="100" />
<param name="obs_lg" value="200" />
<param name="obs_hg" value="255" />
<param name="obs_lr" value="200" />
<param name="obs_hr" value="255" />
</node>
</launch>
새로운 소스 파일이 추가되어 새로운 노드 파일을 생성하므로, CMakeLists.txt 파일에 아래와 같이 추가한다.
# find opencv for cmake
find_package( OpenCV REQUIRED )
## coverage proc node
add_executable( coverage_proc_node src/coverage_proc.cpp )
## Specify libraries to link a library or executable target against
target_link_libraries( coverage_proc_node
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)
이렇게 수정후에 아래와 같이 catkin_make 한다.
$ cd ~/catkin_ws
$ catkin_make
아래와 같이 launch 파일을 이용하여 coverage 계산 노드를 실행한다.
$ roslaunch mobile_robot_sim coverage_proc.launch
위와 같이 실행하면 터미널에 아래와 같이 장애물 영역, 전체 면적, covered 면적등이 표시되고, 아래와 같은 이미지가 표시된다. 이미지는 왼쪽 위부터 시계방향으로 각각 input 이미지, 장애물 이미지, contour 이미지, coverage 이미지를 나타낸다. 장애물 이미지를 보면, 내부가 비어있는 장애물이 있어서, 정확한 장애물 영역 계산은 계산되지 않았다.
[ INFO] [1455443854.597659375]: obstacle area: 45831
[ INFO] [1455443854.676854431]: max area: 211152.00
[ INFO] [1455443854.681730820]: covered area: 25143
[ WARN] [1455443854.681831391]: coverage ratio: 15.21 [%]
