Mobile 로봇 Simulation

주행 클래스 생성하기

주행 클래스를 만들고 로봇을 움직여 위치를 출력해 본다.

토픽결과물: 로봇이 간단히 주행할 수 있는 클래스가 만들어 집니다.

Gazebo 주행 시뮬레이션을 하기 위한 class를 생성합니다. 편의상 cpp 파일 하나에 코딩합니다. 단, class와 관련성이 떨어지는 함수는 별도의 utils.hpp 파일에 코딩합니다.

설명 동영상

 

utils.hpp

몇 가지 편의 함수들을 정의하여 사용합니다.

#include <math.h>
#include <cstdlib>
#include <sys/time.h>

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

/*
 * measure time in milliseconds
 */
double mstime()
{
  timeval tv;
  gettimeofday(&tv,NULL);
  long long ts = tv.tv_sec;
  ts *= 1000000;
  ts += tv.tv_usec;
  return (double)ts*.001;
}

/*
 * set a seed for random number generation
 * @param seed the seed
 */
void setSeed( const unsigned long int _seed )
{
  srand48( _seed );
}

/*
 * sample from the given gaussian distribution (zero mean, std)
 * @param std standard deviation
 * @return resulting sample
 */
double sampleGaussian( const double _std )
{
  double x1, x2, w;
  double r;
  do
  {
    do
    {
      r= drand48();
    }while( r == 0.0f );
    x1= 2.0f * r - 1.0f;
    do
    {
      r= drand48();
    }while( r == 0.0f );
    x2= 2.0f * drand48() - 1.0f;
    w= x1*x1 + x2*x2;
  }while( w > 1.0f || w==0.0f );

  return( _std * x2 * sqrt(-2.0*log(w)/w) );
}

mobile_robot_sim.cpp 파일을 아래와 같이 작성합니다.

Include files

ROS 의존 패키지들을 인클루드 합니다.

// util functions
#include "utils.hpp"

//////////// ros related
// ros
#include <ros/ros.h>
// ros package to access package directory
#include <ros/package.h>
// laser scan message
#include <sensor_msgs/LaserScan.h>
// tf2
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
// tf2 matrix
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
//
#include <geometry_msgs/TransformStamped.h>
// move base to move the kobuki
#include <move_base_msgs/MoveBaseAction.h>
// velocity message
#include <geometry_msgs/Twist.h>
// 2d pose
#include <geometry_msgs/Pose2D.h>
// ground truth pose message
#include <nav_msgs/Odometry.h>

structs and enums

편의를 위한 구조체와 enum 들을 선언합니다. 이 중 몇가지는 장애물 회피 및 Wall Following에 사용될 것입니다.

//////// 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,
};

// obstacle state strings
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"
};

// 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,
};

MRS Paramters

Mobile Robot Sim 패키지에서 사용하는 파라미터는 별도의 struct에 모아서 사용합니다. 이렇게 함으로써 코드상에서 어떤 변수가 파라미터로 launch 파일에서 수정가능한지 알 수 있습니다.

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

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

MRS class

Mobile Robot Sim 패키지의 클래스 본체입니다. 여기서도 장애물 회피 및 Wall Following 관련 멤버들이 먼저 소개되고 있습니다. 본 토픽에서는 ground truth pose 메세지 수신, hokuyo scan 수신, odometry pose 획득 등만 설명됩니다. 이 때, tf가 아니라 tf2가 사용됨을 확인할 수 있습니다.

/*
 * Mobile Robot Simulation (MRS) class
 */
class MRS
{
public:
  /// constructor
  MRS();
  /// destructor
  ~MRS();
  /// initialize
  void init( const geometry_msgs::Pose2D& _init_pose );
  /// run by setting topics and publishers and subscribers
  void run();
  /// initialize scan data
  bool initScan( const sensor_msgs::LaserScanConstPtr& _scan );
  /// scan message callback
  void scanCB( const sensor_msgs::LaserScanConstPtr& _scan );
  /// ground truth pose message callback
  void groundTruthCB( const nav_msgs::Odometry::ConstPtr& _odom_msg );
  /// get odometry pose
  bool getOdomPose( const ros::Time& _stamp, 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 );

private:
  //////// ros
  /// ros node handle
  ros::NodeHandle ros_nh_;

  //////// message topics
  ros::Subscriber scan_sub_;

  /////// tf2
  /// tf2 buffer
  boost::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
  /// tf2 listener
  boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
  /// tf2 broadcaster
  tf2_ros::TransformBroadcaster *tf2_broadcaster_;

  /////// ground truth
  /// initial pose
  geometry_msgs::Pose2D init_pose_;
  /// ground truth pose topic subscriber
  ros::Subscriber gt_pose_sub_;
  /// ground truth pose
  geometry_msgs::Pose2D gt_pose_;
  /// transformed ground truth pose publisher
  ros::Publisher gt_pose_pub_;

  /////// move base
  ros::Publisher vel_pub_;

  /////// scan
  /// min angle of scan in radians
  double scan_angle_min_;
  /// max angle of scan in radians
  double scan_angle_max_;
  /// angle increment of scan in radians
  double scan_angle_inc_;

  /// count the scans
  int scan_count_;

  //////// parameters
  MRSParameters params_;

  /// the first scan receiving flag
  bool got_first_scan_;

  //////// 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_;
};	// end of MRS class

클래스 생성자, 소멸자

/*
 * Constructor
 */
MRS::MRS()
: got_first_scan_(false)
{
  // reset the obstacle bit set
  obs_bits_.reset();
  // initial driving state is normal drive
  driving_state_= DRIVE;
}

/*
 * Destructor
 */
MRS::~MRS()
{
}

파라미터 초기화 멤버함수

launch 파일에서 파라미터를 읽어서 초기화 합니다.

/*
 * Initialize parameters
 */
void MRS::init( const geometry_msgs::Pose2D& _init_pose )
{
  // set the initial pose
  init_pose_= _init_pose;

  // get parameters. set default values unless the values exist
  ros::NodeHandle ros_nh("~");

  ros_nh.param<std::string>( "odom_frame", params_.odom_frame, "odom" );
  ros_nh.param<std::string>( "base_frame", params_.base_frame, "base_link" );
  ros_nh.param<std::string>( "scan_frame", params_.scan_frame, "hokuyo_frame" );

  ros_nh.param<double>( "max_range", params_.max_range, 3.0 );
  ros_nh.param<double>( "min_range", params_.min_range, 0.0 );
  ros_nh.param<double>( "range_noise_ratio", params_.range_noise_ratio, 0.0 );
  ros_nh.param<int>("throttle_scans", params_.throttle_scans, 1 );

  ros_nh.param<double>( "right_section_angle", params_.section_angles[RIGHT], -80.0 );
  ros_nh.param<double>( "front_section_min_angle", params_.section_angles[FLEFT], -30.0 );
  ros_nh.param<double>( "front_section_max_angle", params_.section_angles[FRIGHT], 30.0 );
  ros_nh.param<double>( "left_section_angle", params_.section_angles[LEFT], 80.0 );
  ros_nh.param<int>( "side_section_interval", params_.side_section_interval, 4 );

  ros_nh.param<double>( "obs_dist_thresh_r", params_.obs_dist_threshs[RIGHT], 0.3 );
  ros_nh.param<double>( "obs_dist_thresh_fr", params_.obs_dist_threshs[FRIGHT], 0.3 );
  ros_nh.param<double>( "obs_dist_thresh_fl", params_.obs_dist_threshs[FLEFT], 0.3 );
  ros_nh.param<double>( "obs_dist_thresh_l", params_.obs_dist_threshs[LEFT], 0.3 );
  ros_nh.param<double>( "obs_dist_thresh_emerg", params_.obs_dist_threshs[OBS_SECTION_SIZE], 0.1 );

  ros_nh.param<int>( "obs_count_thresh_r", params_.obs_count_threshs[RIGHT], 1 );
  ros_nh.param<int>( "obs_count_thresh_fr", params_.obs_count_threshs[FRIGHT], 1 );
  ros_nh.param<int>( "obs_count_thresh_fl", params_.obs_count_threshs[FLEFT], 1 );
  ros_nh.param<int>( "obs_count_thresh_l", params_.obs_count_threshs[LEFT], 1 );
  ros_nh.param<int>( "obs_count_thresh_emerg", params_.obs_count_threshs[OBS_SECTION_SIZE], 1 );

  ros_nh.param<double>( "drive_linvel", params_.drive_linvel, 0.2 );
  ros_nh.param<double>( "obs_approach_linvel", params_.obs_approach_linvel, 0.1 );
  ros_nh.param<double>( "obs_avoid_angvel", params_.obs_avoid_angvel, 0.3 );

  ros_nh.param<double>( "desired_wf_dist", params_.desired_wf_dist, 0.2 );
  ros_nh.param<double>( "wf_slow_pgain", params_.wf_slow_pgain, 1.0 );
  ros_nh.param<double>( "wf_slow_pgain", params_.wf_slow_dgain, 1.0 );
}

ROS Subsriber, Publisher 설정

publisher와 subscriber를 설정합니다. 또한 tf2를 초기화 하여 tf2로 부터 odometry pose를 추출할 수 있게 합니다.

/*
 * Run and set publishers and subscribers.
 */
void MRS::run()
{
  int queue_size= 50;

  // subscribe the scan message
  scan_sub_= ros_nh_.subscribe( "scan", queue_size, &MRS::scanCB, this );

  // subscribe the ground truth pose message
  gt_pose_sub_= ros_nh_.subscribe( "ground_truth_pose", queue_size, &MRS::groundTruthCB, this );
  // publish transformed ground truth pose. @note "gt_pose" topic name is not working, why?
  gt_pose_pub_= ros_nh_.advertise<geometry_msgs::PoseStamped>( "ground_truth_pose2", queue_size, true );

  // velocity publisher
  vel_pub_= ros_nh_.advertise<geometry_msgs::Twist>( "cmd_vel", queue_size, true );

  // initialize tf2
  tf2_buffer_.reset(new tf2_ros::Buffer());
  tf2_listener_.reset(new tf2_ros::TransformListener( *tf2_buffer_ ));
  tf2_broadcaster_= new tf2_ros::TransformBroadcaster(); 
}

로봇 속도 publish

로봇의 선속도, 각속도를 메시지로 퍼블리쉬 합니다. 속도 메시지는 geometry_msgs::Twist를 사용합니다.

/*
 * Publish velocity message.
 * @param v linear velocity in m/s
 * @param w angular velocity in rad/s
 */
void MRS::publishVelocity( const double _v, const double _w )
{
  geometry_msgs::Twist vel;
  vel.linear.x= _v;
  vel.angular.z = _w;
  vel_pub_.publish( vel );
}

Scan 데이터 초기화

수신한 hokuyo 스캔 데이터로 스캔 관련 변수를 초기화 합니다.

/*
 * Initialize the scan data with the fist scan data
 * @param scan the first scan data to initialize the map
 */
bool MRS::initScan( const sensor_msgs::LaserScanConstPtr& _scan )
{
#if __DEBUG__
  ROS_WARN( "Initializing scan..." );
#endif

  // sensor parameters
  scan_angle_min_= _scan->angle_min;
  scan_angle_max_= _scan->angle_max;
  scan_angle_inc_= _scan->angle_increment;
  ROS_INFO( "Scan angles top down in scan frame: min: %.3f, max: %.3f, inc: %.3f",
      scan_angle_min_, scan_angle_max_, scan_angle_inc_ );

#if __DEBUG__
  ROS_INFO( "Scan initialization completed" );
#endif

  // return success
  return true;
}

Ground Truth Pose 콜백함수

ground_truth_pose topic을 수신합니다. 여기서 메시지로부터 수신한 ground truth pose는 gazebo 좌표계로 표시되므로, 이것을 로봇의 초기 위치 기준 좌표계로 변경시켜야 합니다. 즉, 로봇 초기 위치의 ground truth pose는 (0, 0, 0) 되도록 변환해야 합니다.

/*
 * Callback for ground truth pose.
 * Apply the initial pose to the ground truth pose.
 *
 */
void MRS::groundTruthCB( const nav_msgs::Odometry::ConstPtr& _odom_msg )
{
  //// compute the frame transform. [G] to [R0]. [G] is ground truth frame. [R0] is the initial robot frame.
  // The transform is R0_T_G.
  static double cos_theta= cos(-init_pose_.theta);
  static double sin_theta= sin(-init_pose_.theta);
  // need to transform the initial translation
  static double init_x= cos_theta*(-init_pose_.x) - sin_theta*(-init_pose_.y);
  static double init_y= sin_theta*(-init_pose_.x) + cos_theta*(-init_pose_.y);
  // transform the ground truth to the R
  double x= cos_theta*_odom_msg->pose.pose.position.x - sin_theta*_odom_msg->pose.pose.position.y;
  double y= sin_theta*_odom_msg->pose.pose.position.x + cos_theta*_odom_msg->pose.pose.position.y;
  gt_pose_.x= x + init_x;
  gt_pose_.y= y + init_y;

  // build tf quaternion
  tf2::Quaternion q( _odom_msg->pose.pose.orientation.x, _odom_msg->pose.pose.orientation.y, _odom_msg->pose.pose.orientation.z, _odom_msg->pose.pose.orientation.w );
  // build tf matrix from the quaternion
  tf2::Matrix3x3 m(q);
  // get angles
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  // ground truth of yaw
  gt_pose_.theta= yaw - init_pose_.theta;

  // build ground pose message
  geometry_msgs::PoseStamped gt_pose_msg;
  gt_pose_msg.header.frame_id= params_.odom_frame;
  gt_pose_msg.header.stamp= _odom_msg->header.stamp;
  gt_pose_msg.pose.position.x= gt_pose_.x;
  gt_pose_msg.pose.position.y= gt_pose_.y;
  gt_pose_msg.pose.position.z= 0.0;
  tf2::Quaternion q2;
  q2.setRPY( 0., 0., gt_pose_.theta );
  gt_pose_msg.pose.orientation.x= q2.x();
  gt_pose_msg.pose.orientation.y= q2.y();
  gt_pose_msg.pose.orientation.z= q2.z();
  gt_pose_msg.pose.orientation.w= q2.w();

  // publish the ground truth pose message
  gt_pose_pub_.publish( gt_pose_msg );

#if __DEBUG__
  ROS_WARN( "ground truth pose: %.2f, %.2f, %.2f", gt_pose_.x, gt_pose_.y, gt_pose_.theta );
#endif
}

프로그램을 실행했을 때 아래와 같이 초기 위치값이 있을 때, ground truth pose 메시지 출력은 (0, 0, 0)이 되어야 합니다.

[ WARN] [1455280186.374946772]: initial pose x, y, th: -2.00, -2.70, 0.00
[ INFO] [1455280195.149926468, 934.408000000]: odom_pose: 0.00, 0.00, -0.00
[ WARN] [1455280195.155493656, 934.410000000]: ground truth pose: 0.00, 0.00, -0.00

Odometry Pose 추출

tf2에서 base_frame to odom_frame 값을 추출합니다. 즉, odom frame기준으로 base_frame (여기서는 base_footprint)의 pose를 추출하게 됩니다.

/*
 * Get odom pose from the scan pose
 * @param stamp ros time of the scan
 * @param odom_pose resulting pose
 */
bool MRS::getOdomPose( const ros::Time& _stamp, geometry_msgs::Pose2D *_odom_pose )
{
  geometry_msgs::TransformStamped odom_msg;
  try
  {
    odom_msg= tf2_buffer_->lookupTransform( params_.odom_frame, params_.base_frame, _stamp );
  }
  catch( tf2::TransformException &ex )
  {
    ROS_WARN( "Failed to compute odom pose, skipping scan (%s)", ex.what() );
    return false;
  }

  // get the odometry pose: actually it is the pose of scan frame based on the odom_frame_
  _odom_pose->x= odom_msg.transform.translation.x;
  _odom_pose->y= odom_msg.transform.translation.y;
  //// get the yaw
  // build tf2 quaternion
  tf2::Quaternion q( odom_msg.transform.rotation.x, odom_msg.transform.rotation.y, odom_msg.transform.rotation.z, odom_msg.transform.rotation.w );
  // build tf2 matrix
  tf2::Matrix3x3 m(q);
  // get angles
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  // set the yaw
  _odom_pose->theta= yaw;

#if __DEBUG__
  ROS_INFO( "odom_pose: %.2f, %.2f, %.2f", _odom_pose->x, _odom_pose->y, _odom_pose->theta );
#endif
  // return true since the odom pose has been obtained
  return true;
}

Scan 콜백 함수

hokuyo 센서가 송신하는 scan 메시지를 수신합니다. 추가적으로 수신한 센서값에 noise를 추가할 수 있게 구현되었습니다.

/*
 * scan callback
 */
void MRS::scanCB( const sensor_msgs::LaserScanConstPtr& _scan )
{
  // increase the scan count
  scan_count_++;
  // handle only if the throttle condition is met
  if( scan_count_%params_.throttle_scans != 0 )
    return;

  // initialize scan data
  if( !got_first_scan_ )
  {
    // initialize scan
    initScan( _scan );
    // now we got the first scan
    got_first_scan_= true;
    return;
  }

  // get odometry pose
  geometry_msgs::Pose2D odom_pose;
  getOdomPose( _scan->header.stamp, &odom_pose );

  // filter range readings and detect obstacles
  ScanRanges ranges;
  for( int i=0; i<_scan->ranges.size(); i++ )
  {
    // filter out (set max range) shorter or farther ranges 
    if( _scan->ranges[i] < _scan->range_min || _scan->ranges[i] > params_.max_range || std::isnan(_scan->ranges[i]) )
      ranges.push_back( params_.max_range );
    else
    {
      //// add noise
      // noise std
      double noise_std= _scan->ranges[i]*params_.range_noise_ratio;
      double noisy_scan= _scan->ranges[i] + sampleGaussian( noise_std );
      ranges.push_back( noisy_scan );
    }
  }
}	// end of scanCB

main 함수

main 함수에서는 먼저, 로봇 초기위치에 대한 argument를 parsing합니다. 이 arguments는 mobile robot sim 노드의 실행 인자로 입력받게 됩니다. 이 부분은 아래 launch 파일 부분에서 확인할 수 있습니다. 그 다음 위에서 생성한 클래스의 인스턴스를 생성하여 init(), run() 함수를 호출하고, 제어권을 ROS에게 넘깁니다.

/*
 * main function
 */
int main( int argc, char **argv )
{
  // ros init
  ros::init( argc, argv, "mobile_robot_sim_node" );

  // parse arguments
  ROS_ASSERT( argc > 6 );
  geometry_msgs::Pose2D init_pose;
  if( std::string(argv[1]) == "-init_x" )
    init_pose.x= atof( argv[2] );
  if( std::string(argv[3]) == "-init_y" )
    init_pose.y= atof( argv[4] );
  if( std::string(argv[5]) == "-init_theta" )
    init_pose.theta= atof( argv[6] );
  ROS_WARN( "initial pose x, y, th: %.2f, %.2f, %.2f", init_pose.x, init_pose.y, init_pose.theta );

  // create a MRS instance
  MRS mrs;
  // initialize mrs
  mrs.init( init_pose );
  // ros run
  mrs.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 파일

최상위 launch (turtlebot_hokuyo.launch) 파일으로서 로봇 초기위치 설정, gazebo 노드 실행, mobile robot sim 노드 실행 관련 설정이 포함되어 있습니다. 여기서 mobile robot sim 노드 실행을 위해 별도의 launch 파일을 include하고 있는데, 그 부분은 밑에서 설명합니다.

<!-- 
  gazebo mobile robot simulation: kobuki, asus, hokuyo 2d laser
-->
<launch>

  <!-- scan topic name -->
  <arg name="scan_topic" default="/hokuyo_scan" />

  <!-- gazebo initial pose -->
  <arg name="init_x" default="-2.0" />  <!-- -2.0 -->
  <arg name="init_y" default="-2.7" />  <!-- -2.7 -->
  <arg name="init_theta" default="0.0" /> <!-- 0.0 -->
  
  <!-- gazebo setting for turtlebot -->
  <arg name="base"        default="kobuki"/> <!-- create, roomba -->
  <arg name="battery"     default="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>  <!-- /proc/acpi/battery/BAT0 --> 
  <arg name="stacks"      default="hexagons"/>  <!-- circles, hexagons --> 
  <arg name="3d_sensor"   value="asus"/>  <!-- asus_xtion_pro --> 
  <arg name="2d_laser"    value="hokuyo"/> <!-- hokuyo 2d laser -->
  <arg name="world_file"  value="$(find mobile_robot_sim)/worlds/sim_simple.world"/>  

  <!-- include gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="use_sim_time" value="true"/>
    <arg name="debug" value="false"/>
    <arg name="world_name" value="$(arg world_file)"/>
  </include>

  <include file="$(find mobile_robot_sim)/launch/includes/$(arg base)_hokuyo.launch.xml">
    <arg name="base" value="$(arg base)"/>
    <arg name="stacks" value="$(arg stacks)"/>
    <arg name="3d_sensor" value="$(arg 3d_sensor)"/>
    <arg name="2d_laser" value="$(arg 2d_laser)"/>
    <arg name="x" value="$(arg init_x)"/>
    <arg name="y" value="$(arg init_y)"/>
    <arg name="theta" value="$(arg init_theta)"/> 
  </include>

  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>
  
  <!-- include mobile robot sim launch file -->   
  <include file="$(find mobile_robot_sim)/launch/includes/mrs.launch.xml"> 
    <arg name="scan_topic" value="$(arg scan_topic)" />
    <arg name="init_x"     value="$(arg init_x)"/>
    <arg name="init_y"     value="$(arg init_y)"/>
    <arg name="init_theta" value="$(arg init_theta)"/> 
  </include>
</launch>

mobile robot sim 노드 관련 파라미터

mobile robot sim 노드 관련 파라미터를 별도의 파일 (mrs.launch.xml)로 만들어서 관리합니다.

						

댓글

댓글 본문
작성자
비밀번호
graphittie 자세히 보기