Multi-robot Simulation

Kobuki Robot 클래스 생성하기

토픽 결과물: Mobile 

 

ROS Gazebo 강좌8에서 로봇(터틀봇) 한 대에 대해서 장애물 회피 주행 코드를 생성하였는데, 이번 강좌에서는 4대의 로봇(Kobuki, 거북이)의 장애물 회피 주행 코드를 작성한다. 이번 강좌에서는 각 로봇 주행에 대한 부분을 별도의 클래스로 만들고, 여러 대의 로봇 객체를 생성하고 관리하는 ROS Wrapping 클래스를 만들 것이다. 로봇 주행에 대한 클래스는 ROS와 무관하게 작성할 것이다. 단 편의상 ROS의 데이터 타입을 위해 ROS 인크루드는 몇 개 사용할 것이다.

읽기 전에

본 강좌는 강좌 1. Gazebo Mobile Robot Simulation의 Wall Following 코드 작성하기의 내용을 수정하여 작성한 것이다. 강좌 1에서는 한대의 Turtlebot에 대해서 장애물 회피를 구현하였고, 이번 강좌는 4대의 Kobuki에 대해서 장애물 회피를 구현하였다. 주의할 것은 터틀봇과 거북이의 스캔 데이터의 인덱싱이 다르다. 터틀봇은 로봇의 오른쪽부터 인덱싱되고, 거북이는 왼쪽부터 인덱싱된다. 4대 구현에 있어서 Gazebo 쪽인지 구현 코드 쪽인지 불안정하여 에러가 발생할 수 있다. 본 강좌를 읽기전에 Wall Following 코드 작성하기의 내용을 훑어 보는 것이 좋다.

본 강좌에서는 로봇 클래스를 생성하는 부분에 대해서 설명한다.

robot.h

cpp파일에 모든 코드를 넣기에는 분량이 있어서 별도의 클래스 헤더파일을 만든다. 로봇 클래스는 robot.h와 robot.cpp로 되어 있다. 대부분의 내용이 Wall Following 코드 작성하기의 MRS 클래스와 유사하다.

#ifndef __ROBOT_H__
#define __ROBOT_H__

#include <bitset>

#include <ros/ros.h>

// velocity message
#include <geometry_msgs/Twist.h>
// 2d pose
#include <geometry_msgs/Pose2D.h>
// ground truth pose message
#include <nav_msgs/Odometry.h>

#define DEG2RAD(x) (M_PI/180.0)*x

//////// struct and enum
// range vector of a laser scan
typedef std::vector<double> ScanRanges;

// obstacle sections: four sections for convenience
enum
{
  RIGHT=0,
  FRIGHT=1, // front right
  FLEFT=2,  // front left
  LEFT=3,
  OBS_SECTION_SIZE,
};

//// obstacle states
// left wall (L), right wall (R), front left (FL), front right (FR), front (F)
// order: left -> front -> right
enum
{
  OBS_CLEAR= 0,
  // --------- side sect line -----------
  OBS_L= 1,  // left wall only,  1000
  OBS_R= 2,  // right wall only, 0001
  OBS_L_R= 3, // left wall and right wall, 1001
  // -------- front sect line -----------
  OBS_FL= 4,  // front left only, 0100
  OBS_FR= 5,  // front right only, 0010
  OBS_F= 6, // front, 0110
  OBS_L_FL= 7, // left wall and front left, 1100
  OBS_FR_R= 8, // front right and right wall, 0011
  OBS_L_F= 9,  // right wall clear, 1110, 1010
  OBS_F_R= 10, // left wall clear, 0111, 0101
  OBS_ALL= 11,  // left wall, right wall, and either front left or front right, 1111, 1101, 1011
  OBS_STATE_SIZE,
  // --------------------------------------
  OBS_FRONT_SECT= 3,
};


// driving states
enum
{
  DRIVE= 0,           // normal drive
  DRIVE_APPROACH= 1, // approach drive
  DRIVE_AVOID= 2,    // avoid drive
  DRIVE_WF= 3,       // wall following drive
};

// wall following directions
enum
{
  WF_LEFT= -1,
  WF_RIGHT= 1,
};

// obstacle state strings
static const char *OBS_STRINGS[]=
{
  "OBS_CLEAR", "OBS_L", "OBS_R", "OBS_L_R", "OBS_FL", "OBS_FR", "OBS_F", "OBS_L_FL", "OBS_FR_R",
  "OBS_L_F", "OBS_F_R", "OBS_ALL", "OBS_STATE_SIZE"
};

// Mobile Robot Simulation Parameters
struct MRSParams
{
  /// odometry frame
  std::string odom_frame;
  /// base frame: currently base_footprint
  std::string base_frame;
  /// scan frame: hokuyo sensor
  std::string scan_frame;

  /// scan angle min
  double scan_angle_min;
  /// scan angle max
  double scan_angle_max;
  /// scan angle increment
  double scan_angle_inc;
  /// max sensing distance
  double max_range;
  /// min sensing distance
  double min_range;
  /// ratio to add noise to the range data
  double range_noise_ratio;
  /// regulate the scan processing frequency
  int throttle_scans;
  /// angles of rays indicating the sections in radians
  double section_angles[OBS_SECTION_SIZE];
  /// obstacle distance threshold including the approach threshold
  double obs_dist_threshs[OBS_SECTION_SIZE+1];
  /// obstacle count threshold
  int obs_count_threshs[OBS_SECTION_SIZE];
  /// offset indices from the side ray
  int side_section_interval;
  /// approaching linear velocity
  double obs_approach_linvel;
  /// avoiding angular velocity
  double obs_avoid_angvel;
  /// normal driving linear velocity
  double drive_linvel;
  /// desired wall following distance
  double desired_wf_dist;
  /// wall following control p gain
  double wf_slow_pgain;
  /// wall following control d gain
  double wf_slow_dgain;
};

// robot class
class Robot
{
public:
  /// constructor
  Robot();
  /// destructor
  ~Robot();
  /// set parameters
  void setParams( const MRSParams &_params );
  /// run by setting topics and publishers and subscribers
  void run();
  /// process a scan and return the commanded velocity
  void processScan( const ScanRanges &_ranges, geometry_msgs::Pose2D _odom_pose, geometry_msgs::Twist* _velocity );
  /// set the ground truth pose
  void setGroundTruthPose( const geometry_msgs::Pose2D _gt_pose );
  /// set odometry pose
  bool setOdomPose( const geometry_msgs::Pose2D _odom_pose );

  ////// move base
  /// publish robot velocities
  void publishVelocity( const double _v, const double _w );
  /// scan obstacle sections
  void scanObsSections( const ScanRanges &_ranges );
  /// determine obstacle state based on the obstacle scan result
  bool determineObsState( const ScanRanges& _ranges );
  /// decide wall following direction
  /// determine wall following direction based on the obstacle state
  int determineWFDirection();
  /// do wall following
  void doWallFollowing( const int _wf_dir, const double _target_dist, geometry_msgs::Twist* _velocity );

private:
  //////// parameters
  MRSParams params_;
  /// setting parameters flag
  bool params_set_;

  /////// ground truth
  /// initial pose
  geometry_msgs::Pose2D init_pose_;
  /// ground truth pose
  geometry_msgs::Pose2D gt_pose_;


  /////// scan
  /// count the scans
  int scan_count_;

  //////// obstacle [obs]
  /// obstacle distance
  double obs_dist_[OBS_SECTION_SIZE];
  /// obstacle count
  int obs_count_[OBS_SECTION_SIZE];
  /// bit0: right, bit1: front_right, bit2: front_left, bit3: left
  std::bitset<4> obs_bits_;
  /// obstacle state
  int obs_state_;
  /// driving state
  int driving_state_;
  /// old distance error
  double old_dist_error_;

};
#endif  # end of #ifndef __ROBOT_H__

robot.cpp

Robot 클래스 생성자, 소멸자

#include "multi_robots_sim/robot.h"

Robot::Robot()
: params_set_(false)
, scan_count_(0)
{
  // reset the obstacle bit set
  obs_bits_.reset();
  // initial driving state is normal drive
  driving_state_= DRIVE;
}

Robot::~Robot()
{
}

로봇 주행에 관련된 parameter 설정

ROS Wrapping 클래스에서 아래 함수를 호출하여 각 로봇의 파라미터를 설정하게 된다.

/*
 * Set parameters
 */
void Robot::setParams( const MRSParams &_params )
{
  // set parameters
  params_= _params;
  params_set_= true;
}

Scan 처리 함수

ROS Wrapping 클래스에서 각 로봇의 scan topic를 수신하여 각 로봇별로 scan data를 처리하는 함수이다. scan 데이터에 따라서 장애물 상태를 파악하고, 이동해야 할 속도를 계산해 낸다.

/*
 * Process a scan
 */
void Robot::processScan( const ScanRanges &_ranges, geometry_msgs::Pose2D _odom_pose, geometry_msgs::Twist* _velocity )
{
  // check if the parameters are set
  if( !params_set_ )
  {
    ROS_WARN( "paramseters are not set" );
    return;
  }

  // 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

  // determine wall following direction or choose one
  int wf_dir= determineWFDirection();
  // jump to a branch based on the driving state
  switch( driving_state_ )
  {
  case DRIVE:
    // obstacle detected?
    if( detected )
    {
      // check if either side of the robot is close to a wall
      if( obs_dist_[LEFT] <= params_.desired_wf_dist || obs_dist_[RIGHT] <= params_.desired_wf_dist )
      {
        // transit to wall following driving
        driving_state_= DRIVE_WF;
#if __DEBUG__
        ROS_WARN( "[DRIVE] --> [DRIVE_WF]" );
#endif
        break;
      }
      // front obstacle?
      if( obs_state_ > OBS_FRONT_SECT )
      {
        // transit to approaching driving
        driving_state_= DRIVE_APPROACH;
        // approach slowly
        _velocity->linear.x= params_.obs_approach_linvel;
        _velocity->angular.z= 0.0;
#if __DEBUG__
        ROS_WARN( "[DRIVE] --> [DRIVE_APPROACH]" );
#endif

      }
      // side obstacle
      else
      {
        // transit to wall following driving
        driving_state_= DRIVE_WF;
#if __DEBUG__
        ROS_WARN( "[DRIVE] --> [DRIVE_WF]" );
#endif
      }
    }
    else
    {
      // drive normally
      _velocity->linear.x= params_.drive_linvel;
      _velocity->angular.z= 0.0;
    }
    break;
  case DRIVE_APPROACH:
#if __DEBUG__
    ROS_INFO( "[approach] param thresh: %.2f, obs_dist: %.2f", params_.obs_dist_threshs[OBS_SECTION_SIZE], obs_dist_[FLEFT] );
#endif
    // check if either side of the robot is close to a wall
    if( obs_dist_[LEFT] <= params_.desired_wf_dist || obs_dist_[RIGHT] <= params_.desired_wf_dist )
    {
      // transit to avoiding driving
      driving_state_= DRIVE_AVOID;
      // avoid obstacle by rotating the robot
      _velocity->linear.x= 0.0;
      _velocity->angular.z= wf_dir*params_.obs_avoid_angvel;
#if __DEBUG__
      ROS_WARN( "[DRIVE_APPROACH] --> [DRIVE_AVOID]" );
#endif
      break;
    }
    // approaching done? checking the front sections
    if( obs_dist_[FLEFT] <= params_.obs_dist_threshs[OBS_SECTION_SIZE] ||
        obs_dist_[FRIGHT] <= params_.obs_dist_threshs[OBS_SECTION_SIZE] )
    {
      // transit to avoiding driving
      driving_state_= DRIVE_AVOID;
      // let the robot rotate
      _velocity->linear.x= 0.0;
      _velocity->angular.z= wf_dir*params_.obs_avoid_angvel;
#if __DEBUG__
      ROS_WARN( "[DRIVE_APPROACH] --> [DRIVE_AVOID]" );
#endif
    }
    else
    {
      // approach slowly
      _velocity->linear.x= params_.obs_approach_linvel;
      _velocity->angular.z= 0.0;
    }
    break;
    // rotate until no front obstacle is detected
  case DRIVE_AVOID:
    // keep rotating while front obstacle exists
    if( obs_dist_[FLEFT] <= params_.obs_dist_threshs[FLEFT] ||
        obs_dist_[FRIGHT] <= params_.obs_dist_threshs[FRIGHT] )
    {
      // rotate the robot just in case ( robot is already rotating)
      _velocity->linear.x= 0.0;
      _velocity->angular.z= wf_dir*params_.obs_avoid_angvel;
      break;
    }
    // now no front obstacle
    // transit to wf
    driving_state_= DRIVE_WF;
    // clear the error distance
    old_dist_error_= 0.0;
#if __DEBUG__
    ROS_WARN( "[DRIVE_AVOID] --> [DRIVE_WF]" );
#endif
    break;
  case DRIVE_WF:
    // no obstacle?
    if( !detected )
    {
      // transit to normal driving state
      driving_state_= DRIVE;
#if __DEBUG__
      ROS_WARN( "[DRIVE_WF] --> [DRIVE]" );
#endif
      // exit the wall following state
      break;
    }
    else
    {
      // check if either side of the robot is close to a wall.
      if( obs_dist_[LEFT] <= params_.desired_wf_dist-0.2 || obs_dist_[RIGHT] <= params_.desired_wf_dist-0.2 )
      {
        doWallFollowing( wf_dir, params_.desired_wf_dist, _velocity );
        break;
      }

      // front obstacle?
      if( obs_dist_[FLEFT] <= params_.obs_dist_threshs[FLEFT] ||
          obs_dist_[FRIGHT] <= params_.obs_dist_threshs[FRIGHT] )
      {
        // transit to approaching driving
        driving_state_= DRIVE_APPROACH;
        // approach slowly
        _velocity->linear.x= params_.obs_approach_linvel;
        _velocity->angular.z= 0.0;
#if __DEBUG__
        ROS_WARN( "[DRIVE_WF] --> [DRIVE_APPROACH]" );
#endif
        break;
      }
      // keep wall following
      doWallFollowing( wf_dir, params_.desired_wf_dist, _velocity );
    }
    break;
  default:
    ROS_WARN( "Driving state is undefined" );
    break;
  }  // end of switch( driving_state_ )
}

로봇 주변 장애물 검색

scan 데이터로 장애물을 검색하는 함수이다. 이 함수는 장애물 인식 코드 작성하기와 동일하다. 앞서 Turtlebot의 scan과 Kuboki의 scan 데이터의 인덱싱 순서가 다르다고 했는데, 아래 코드는 그대로 두고, scan callback에서 순서를 바꾸어 줄 것이다. 또한, Turtlebot때와 달리 Kobuki의 경우 Laser센서가 로봇 중심에 부착되어 있기 때문에 장애물 검색시에 사용되는 threshold 값을 조정해야 한다. 이것은 launch 파일의 parameter로 할 것이다.

/*
 * Scan four obstacle sections for detecting obstacles.
 * Four sections: right, front right, front left, left
 * @param ranges laser scan range values
 */
void Robot::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 - params_.scan_angle_min)/params_.scan_angle_inc;
  static const int front_max_idx= ( FRONT_MAX_RAD - params_.scan_angle_min)/params_.scan_angle_inc;
  const int mid_idx= int( _ranges.size()/2 );
  ROS_WARN( "front_min_idx: %d, front_max_idx: %d, range size: %d", front_min_idx, front_max_idx, _ranges.size() );
  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 - params_.scan_angle_min)/params_.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 - params_.scan_angle_min)/params_.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;
  }
}

로봇 주변 장애물 상태 판단

검색된 장애물 데이터로 로봇 주변의 장애물 상태를 판단하는 함수이다. 이 함수는 장애물 인식 코드 작성하기와 동일하다.

/*
 * Determine obstacle state based on the obstacle scan result.
 * @param ranges laser scan range values
 */
bool Robot::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;
}

Wall Following 방향 정하기

장애물 상태에 따라 wall following 할 방향을 정하는 함수이다. 이 함수는 Wall Following 코드 작성하기와 동일하다.

/*
 * Determine wall following direction based on the obstacle state.
 * It is called when obstacle is detected.
 */
int Robot::determineWFDirection()
{
  // set left as default direction
  int wf_dir= WF_LEFT;
  // right direction cases
  switch( obs_state_ )
  {
  case OBS_R:
  case OBS_FR:
  case OBS_FR_R:
  case OBS_F_R:
    wf_dir= WF_RIGHT;
    break;
  case OBS_F:
    // choose the side having the shorter distance
    if( obs_dist_[FRIGHT] < obs_dist_[FLEFT] )
      wf_dir= WF_RIGHT;
    break;
  case OBS_ALL:
    // choose the direction to the goal
    break;
  case OBS_L_R:
    break;
  default: // wf_dir= WF_LEFT cases: OBS_L, OBS_L_FL, OBS_L_F, OBS_FL, OBS_CLEAR
    break;
  }

  return wf_dir;
}

Wall Following 제어하기

아래 구현된 wall following은 단순히 벽과의 거리가 desired distance가 되도록 PD 제어하는 함수이다. 이 함수는 Wall Following 코드 작성하기와 동일하다.

/*
 * Do wall following.
 * @param wf_dir wall following direction
 * @param desired_dist desired wall following distance
 */
void Robot::doWallFollowing( const int _wf_dir, const double _desired_dist, geometry_msgs::Twist* _velocity )
{
  // current distance to the wall
  double dist= 0.0;

  // initialize the error distance
  double dist_error= _desired_dist;
//  static double old_dist_error= 0.0;
  // left wall following
  if( _wf_dir == WF_LEFT )
  {
    dist= obs_dist_[LEFT];
    // compute the distance error
    if( dist <= _desired_dist )
    {
      dist_error= (_desired_dist - dist );
    }
    else
    {
      dist_error= -(dist - _desired_dist);
      dist_error*= 0.5;
    }
  }
  // right wall following
  else
  {
    dist= obs_dist_[RIGHT];
    if( dist <= _desired_dist )
    {
      dist_error= -(_desired_dist - dist );
    }
    else
    {
      dist_error= (dist - _desired_dist );
      dist_error*= 0.5;
    }
  }
  // @todo reset old error distance if a new wall following starts
  // by counting the dist having over threshold?

  // gain tuning
  double p_gain= params_.wf_slow_pgain * dist_error;
  double d_gain= params_.wf_slow_dgain * (dist_error - old_dist_error_ );
  double angular_vel= p_gain + d_gain;
  angular_vel *= -1;
  double linear_vel= params_.obs_approach_linvel*2;

#if __DEBUG__
  ROS_WARN( "[WF] dist, desired dist, dist_error, old_dist_error: %.4f, %.4f, %.4f, %.4f", dist, _desired_dist, dist_error, old_dist_error_ );
  ROS_WARN( "[WF] p, d gain: %.5f, %.5f", p_gain, d_gain );
  ROS_WARN( "[WF] linear, angular vel: %.4f, %.4f", linear_vel, angular_vel );
#endif

  // velocity command
  _velocity->linear.x= linear_vel;
  _velocity->angular.z= angular_vel;

  // update the old error distance
  old_dist_error_= dist_error;
}

댓글

댓글 본문
버전 관리
이타인
현재 버전
선택 버전
graphittie 자세히 보기