토픽 결과물: 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;
}
