ROS Gazebo 강좌7
로봇 주변에 장애물이 있는지 없는지 검색하고, 장애물 상태를 판단하는 부분이다. Laser scan 데이터 갯수가 많기 때문에, 여기서는 간단히 4영역으로 나누고, 각 영역에 장애물이 있는지 없는지 판단하여 장애물 상태를 결정하기로 한다.
로봇이 주행할 때 장애물이 있으면 회피해야 한다. 이 때, 모든 Laser scan 데이터를 검사하지 않고, 로봇 주변 영역을 4가지로 나눠서 검색한다. 4가지 영역은 로봇의 오른쪽, 전방 오른쪽, 전방 왼쪽, 왼쪽 영역이다. 영역을 구분짓는 scan 각도는 launch 파일의 파라미터로 설정하였다. 그리고 각 영역에 대해 장애물이라 판단하는 거리값도 파라미터로 설정하였다. 이 두 파라미터를 잘 조정해야 적절한 장애물 회피 동작이 가능하다. 아래 코드에서는 각 영역에 장애물이 감지되면, 즉 거리값이 파라미터의 threshold보다 작으면 해당 영역에 비트를 셋한다. 이 때 std:: bitset을 사용한다. 이렇게 하면 비트단위로 관리하기 편한다. 아래와 같이 mobile_robot_sim.cpp 파일에 장애물 검색 함수를 추가한다.
/*
* Scan four obstacle sections for detecting obstacles.
* Four sections: right, front right, front left, left
* @param ranges laser scan range values
*/
void MRS::scanObsSections( const ScanRanges& _ranges )
{
////// scan the front section
static const double FRONT_MIN_RAD= DEG2RAD( params_.section_angles[FLEFT] );
static const double FRONT_MAX_RAD= DEG2RAD( params_.section_angles[FRIGHT] );
static const int front_min_idx= ( FRONT_MIN_RAD - scan_angle_min_)/scan_angle_inc_;
static const int front_max_idx= ( FRONT_MAX_RAD - scan_angle_min_)/scan_angle_inc_;
const int mid_idx= int( _ranges.size()/2 );
ROS_ASSERT( front_max_idx < _ranges.size() );
ROS_ASSERT( front_min_idx >= 0 );
//// scan front left section
// read the minimum distance threshold for obstacle detection
double min_dist= params_.obs_dist_threshs[FLEFT];
// loop over the indices of front left section
for( int i=front_min_idx; i<mid_idx; i++ )
{
// check front obstacle and update the distance
if( _ranges[i] < min_dist )
{
// set obstacle detection bit
obs_bits_.set( FLEFT );
// update the minimum distance
min_dist= _ranges[i];
}
}
// update the detection count or frequency
if( obs_bits_.test(FLEFT) )
{
// increase the obstacle frequency count
obs_count_[FLEFT]++;
// set the minimum distance
obs_dist_[FLEFT]= min_dist;
}
else
{
// clear the count once no obstacle is detected
obs_count_[FLEFT]= 0;
// set maximum range for no obstacle case
obs_dist_[FLEFT]= params_.max_range;
}
//// scan front right section
// read the minimum distance threshold for obstacle detection
min_dist= params_.obs_dist_threshs[FRIGHT];
// loop over the indices of front right section
for( int i=mid_idx; i<front_max_idx; i++ )
{
// check front obstacle and update the distance
if( _ranges[i] < min_dist )
{
// set obstacle detection bit
obs_bits_.set( FRIGHT );
// update minimum distance
min_dist= _ranges[i];
}
}
// update the detection count
if( obs_bits_.test(FRIGHT) )
{
// increase the obstacle frequency count
obs_count_[FRIGHT]++;
// set the max distance
obs_dist_[FRIGHT]= min_dist;
}
else
{
// clear the count once no obstacle is detected
obs_count_[FRIGHT]= 0;
// set maximum range for no obstacle case
obs_dist_[FRIGHT]= params_.max_range;
}
////// scan the right section
// compute the distance and occupancy for both walls
static const int interval= params_.side_section_interval;
static const double RIGHT_RAD= DEG2RAD( params_.section_angles[RIGHT] );
static const int right_idx= ( RIGHT_RAD - scan_angle_min_)/scan_angle_inc_;
ROS_ASSERT( right_idx-interval >= 0 );
ROS_ASSERT( right_idx+interval < _ranges.size() );
// read the minimum distance threshold for obstacle detection
min_dist= params_.obs_dist_threshs[RIGHT];
// loop over the indices of right section
for( int i=right_idx-interval; i<=right_idx+interval; i++ )
{
if( _ranges[i] < min_dist )
{
// set obstacle detection bit
obs_bits_.set( RIGHT );
// update the minimum distance
min_dist= _ranges[i];
}
}
// update the detection count
if( obs_bits_.test(RIGHT) )
{
// increase the obstacle frequency count
obs_count_[RIGHT]++;
// set the distance
obs_dist_[RIGHT]= min_dist;
}
else
{
// clear the count once no obstacle is detected
obs_count_[RIGHT]= 0;
// set max range for no obstacle case
obs_dist_[RIGHT]= params_.max_range;
}
////// scan left section
static const double LEFT_RAD= DEG2RAD( params_.section_angles[LEFT] );
const int left_idx= ( LEFT_RAD - scan_angle_min_)/scan_angle_inc_;
ROS_ASSERT( left_idx-interval >= 0 );
ROS_ASSERT( left_idx+interval < _ranges.size() );
// read the minimum distance threshold for obstacle detection
min_dist= params_.obs_dist_threshs[LEFT];
// loop over the indices of left section
for( int i=left_idx-interval; i<=left_idx+interval; i++ )
{
if( _ranges[i] < min_dist )
{
// set obstacle detection bit
obs_bits_.set( LEFT );
// update the minimum distance
min_dist= _ranges[i];
}
}
// update the detection count
if( obs_bits_.test(LEFT) )
{
// increase the obstacle frequency count
obs_count_[LEFT]++;
// set the distance
obs_dist_[LEFT]= min_dist;
}
else
{
// clear the count once no obstacle is detected
obs_count_[LEFT]= 0;
// set the max distance
obs_dist_[LEFT]= params_.max_range;
}
}
장애물 검색 함수(scanObsSections)의 결과를 가지고, 로봇 주변의 장애물 상태를 판단한다. 아래 코드에서는 장애물이 검색된 횟수를 파라미터의 threshold와 비교하여 장애물 상태를 최종 결정한다. 즉, 파라미터 값에 따라 장애물이 연속적으로 몇 번 검색되어야 장애물로 인식하여 상태를 결정하게 된다. 장애물 상태는 4 영역에 장애물이 존재하는지에 따라 분리하였다. 주의깊게 볼 점은 아래 코드에서 전방 왼쪽이나 전방 오른쪽이 한쪽만 감지되어도 전방에 장애물이 감지된 것으로 처리했다. 왜냐하면 두 전방 영역중 하나라도 장애물이 있다면 로봇은 전방으로 나아갈 수 없기 때문이다. 아래와 같이 mobile_robot_sim.cpp 파일에 장애물 판단 함수를 추가한다.
/*
* Determine obstacle state based on the obstacle scan result.
* @param ranges laser scan range values
*/
bool MRS::determineObsState( const ScanRanges& _ranges )
{
// clear obstacle bits
obs_bits_.reset();
// clear obstacle state
obs_state_= OBS_CLEAR;
// scan the obstacle sections
scanObsSections( _ranges );
// check the obstacle frequency count if it is really an obstacle
bool detected= false;
// loop over the obstacle sections
for( int i=0; i<OBS_SECTION_SIZE; i++ )
{
// set detection flag if the detection frequency is over the threshold
if( obs_count_[i] > params_.obs_count_threshs[i] )
{
detected= true;
break;
}
}
// return false if not detected
if( !detected )
return false;
//// determine obstacle state
// left section (0x8 = b1000 -> (MSB) L|FL|FR|R (LSB) )
if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0x8) )
{
obs_state_= OBS_L;
}
// right section (0x1 = b0001)
else if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0x1) )
{
obs_state_= OBS_R;
}
// left, right section (0x9 = b1001)
else if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0x9) )
{
obs_state_= OBS_L_R;
}
// front left section (0x4 = b0100)
else if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0x4) )
{
obs_state_= OBS_FL;
}
// front right section (0x2 = b0010)
else if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0x2) )
{
obs_state_= OBS_FR;
}
// front sections (front left and front right) (0x6 = b0110 )
else if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0x6) )
{
obs_state_= OBS_F;
}
// left and front left sections (0x1 = b1100)
else if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0xc) )
{
obs_state_= OBS_L_FL;
}
// front right and right sections (0x3 = b0011)
else if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0x3) )
{
obs_state_= OBS_FR_R;
}
// left and front right sections (0x1 = b1010)
else if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0xa) ||
// left, front left, and front right sections ( 0xe = b1110 )
obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0xe))
{
obs_state_= OBS_L_F;
}
// front left and right sections (0x5 = b0101)
else if( obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0x5) ||
// front left, front right, and right sections (0x7 = b0111)
obs_bits_ == std::bitset<OBS_SECTION_SIZE>(0x7)) // (front left, right), (front left, front right, right)
{
obs_state_= OBS_F_R;
}
// all sections.
// if either front left or front right is obstacle, then the front is obstacle since the robot cannot pass through
else
{
obs_state_= OBS_ALL;
}
// return true since obstacle is detected
return true;
}
앞서 작성한 Scan 콜백 함수 끝부분에 아래와 같은 코드를 삽입하여 로봇이 장애물을 검색하고 장애물 상태를 판단하게 한다. 추가적으로 장애물 상태를 출력해 본다. 이 때, 장애물 판단 거리밖의 scan 데이터에 대해서는 scan의 maximum range를 대입하였다. 즉 장애물 감지 밖의 거리값은 모두 maximum range 값이 표시된다.
/*
* scan callback
*/
void MRS::scanCB( const sensor_msgs::LaserScanConstPtr& _scan )
{
...
// determine obstacle state
bool detected= determineObsState( ranges );
#if __DEBUG__
ROS_INFO( "detected? %d, obstacle state= %s", detected, OBS_STRINGS[obs_state_] );
ROS_INFO( "obs_dist left: %.2f, fleft: %.2f, fright: %.2f, right: %.2f", obs_dist_[LEFT], obs_dist_[FLEFT], obs_dist_[FRIGHT], obs_dist_[RIGHT] );
#endif
}
노드를 실행하기 전에 먼저 catkin_make하여 소스를 빌드한다.
아래와 같이 mobile_robot_sim 패키지의 turtlebot_hokuyo.launch 파일을 실행한다. 그리고 별도의 terminal에서 키보드 조작 노드를 실행하여 로봇을 Gazebo 환경에서 이동시키며 장애물 상태값을 확인한다.
# terminal 1
$ roslaunch mobile_robot_sim turtlebot_hokuyo.launch
# terminal 2 (주의: 로봇을 조종할 때 터미널에 마우스 포커스를 맞추고 key를 눌러야 한다)
$ roslaunch turtlebot_teleop keyboard_teleop.launch
아래 그림은 왼쪽 영역에 장애물이 감지된 상태로써, 왼쪽 거리값은 0.3m이며, 장애물 상태는 왼쪽 영역에만 장애물이 존재하는 OBS_L 가 표시된다.
[ INFO] [1455330420.257113111, 992.286000000]: obs_dist left: 0.30, fleft: 3.00, fright: 3.00, right: 3.00
[ INFO] [1455330420.312715293, 992.324000000]: detected? 1, obstacle state= OBS_L
다음 강좌로 이동