Mobile 로봇 Simulation

Coverage 비율 계산하기

커버리지 계산하기

토픽 결과물: 로봇이 맵을 얼마나 커버했는지 계산할 수 있어요.

이전 토픽에서 생성한 Coverage map을 이용하여 로봇이 cover한 영역의 비율을 계산합니다. Floorplan의 영역을 알 수 없기 때문에, 영상처리 방식으로 Coverage 비율을 계산합니다. 따라서, 실시간으로 비율을 계산하지 않고, coverage 후에 floorplan과 coverage map을 포함하는 이미지를 캡쳐하여 별도의 ROS 노드를 만들어서 계산하는 방식을 택합니다. 영상처리를 위해 OpenCV를 사용합니다. 노드의 주요 부분은 영상처리 부분으로써, ROS가 필요하진 않지만 파라미터 사용 등의 편의를 위해 ROS의 노드 형태로 코딩했습니다. 참고로, 이전 토픽에서 캡쳐한 이미지를 mobile_robot_sim/output/coverage/input.png에 넣어둡니다. 이 경로 및 이름은 파라미터로 launch파일에서 변경가능합니다.

설명 동영상

include files

소스 파일은 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

여기서도 파라미터를 별도의 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;
};

class 선언

클래스 이름은 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()
{
}

init() 함수 생성

파라미터를 읽어서 구조체 변수를 초기화 합니다. 주요 파라미터 변수는 장애물 영역을 계산하는데 필요한 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
}

run() 함수 생성

여기서는 단순히 coverage ratio를 계산하는 함수를 호출하게 됩니다.

/*
 * Run coverage calculation.
 */
void CoverageProc::run()
{
  calculateCoverage();
}

Coverage Ratio 계산 함수 생성

Coverage Ration 계산 방법은 다음과 같습니다.

  1. input 이미지를 color로 연다.
  2. input 이미지에서 장애물 영역을 찾는다. 이 때, OpenCV의 inRange 함수를 사용한다.
  3. 장애물 영역만 나타난 이미지에서 pixel count하여 장애물 영역을 계산한다.
  4. 장애물 영역 이미지에서 contours 찾기를 하여 가장 큰 면적을 갖는 contour를 선택하고, 그 면적을 전체 면적으로 한다.
  5. input 이미지에서 coverage areas를 inRange 함수로 찾는다. coverage areas의 경우는 검은색으로 고정되어 있으므로 별도의 파라미터를 설정하지 않았다.
  6. coverage areas의 pixel count 하여 covered 영역을 계산한다.
  7. 전체 면적에서 장애물 뺀 면적 대비 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
}

main 함수

위에서 선언한 클래스의 객체를 만들고, 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;
}

launch file 생성

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 추가 및 빌드

새로운 소스 파일이 추가되어 새로운 노드 파일을 생성하므로, 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 [%]

 

모듈 정리

지금까지 ROS와 Gazebo simulator를 이용하여 Mobile 로봇이 스스로 벽을 타고 돌아다니는 프로그램을 만들어 봤습니다. 로봇이 그리 잘 돌아다니지는 못하지만, 기본적으로 mobile 로봇 시뮬레이터 작성에 필요한 부분들을 함께 만들어 봤습니다. 보다 완성된 프로그램은 직접 해보시면 더욱 재밌을 것입니다.

다음 모듈 주제는 waypoints navigation, SLAM, Mapping 입니다.

함께 해요~! 

댓글

댓글 본문