토픽 결과물: 로봇이 장애물을 인식하여 피할 수 있게 됩니다.
로봇 주변에 장애물이 있는지 없는지 검색하고, 장애물 상태를 판단하는 부분입니다. 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;
}
ScanCB() 함수 수정
앞서 작성한 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 가 표시됩니다. OBS_L은 Obstacle (OBS)가 Left쪽에 감지되었다는 뜻입니다.
[ 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
