토픽 결과물: Mobile
Kobuki Robot 클래스 생성하기에서 생성한 로봇 클래스에 필요한 scan 데이터 및 odom 데이터를 수신하여 전달하고, 로봇의 속도 명령 topic을 publish하는 역할을 담당하는 ROS wrapping 클래스를 작성한다. 이 클래스는 별도의 헤더파일을 생성하지 않고, multi_robots.sim.cpp라는 파일에 모두 구현한다.
결과 동영상
Include files
구현에 필요한 includes를 작성한다.
#include "multi_robots_sim/robot.h"
#include <string>
// boost thread
#include <boost/thread.hpp>
//// 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>
로봇 개수 설정
로봇 개수를 const 형태로 설정한다. 여기서는 4대의 로봇 객체를 생성한다.
const int NUM_ROBOTS= 4;
ROS Wrapping 클래스 생성
4대의 로봇의 ROS topics 관리를 위한 ROS Wrapping 클래스를 작성한다. 클래스의 이름은 MultiMRS로 한다. MRS는 Mobile Robot Simulation의 약자이다. 주의깊게 볼 부분은 복수의 로봇의 scan/velocity topics을 처리하기 위해 subscriber가 vector 형태로 되어 있다. scan subscriber의 경우 각 로봇의 scan topic을 하나의 scan callback 함수와 연결시키고, callback 함수에서 각 로봇 객체의 scan 처리 함수(processScan)와 연결시킨다.
class MultiMRS
{
public:
/// constructor
MultiMRS();
/// destructor
~MultiMRS();
/// init
void init();
/// run
void run();
/// scan callback
void scanCB( const sensor_msgs::LaserScanConstPtr& _scan, const int _robot_id );
/// init scan
bool initScan( const sensor_msgs::LaserScanConstPtr& _scan );
/// set params for each robot
void setRobotParams( const MRSParams &_params );
/// ground truth pose message callback
// void groundTruthCB( const nav_msgs::Odometry::ConstPtr& _odom_msg );
/// get odometry pose
bool getOdomPose( const ros::Time& _stamp, const int _robot_id, geometry_msgs::Pose2D *_odom_pose );
private:
/// multiple robot instance
Robot robots_[NUM_ROBOTS];
/// Mobile Robot Simulation parameters
MRSParams params_;
/// robot prefix
std::string robot_prefix_;
/// scan topic name
std::string scan_topic_postfix_;
/// odom frame name
std::string odom_frame_postfix_;
/// base frame name
std::string base_frame_postfix_;
/// velocity topic name
std::string vel_topic_postfix_;
//////// ros
/// ros node handle
ros::NodeHandle ros_nh_;
//////// message topics
/// scan subscribers
std::vector<ros::Subscriber> scan_sub_;
/// velocity publishers
std::vector<ros::Publisher> vel_pub_;
/////// 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_;
////// scan
/// the first scan receiving flag
bool got_first_scan_;
};
클래스 생성자 소멸자
scan subscribers와 velocity publishers의 벡터를 로봇 대수만큼 resize하고, 반드시 clear 함수를 호출한다. clear 하지 않을 경우 빈 subscribers/publishers가 벡터에 남아있게 된다.
MultiMRS::MultiMRS()
: got_first_scan_(false)
{
// set the size of subs and pubs
scan_sub_.resize( NUM_ROBOTS );
vel_pub_.resize( NUM_ROBOTS );
// clear the subs and pubs. @note the clearing is necessary.
scan_sub_.clear();
vel_pub_.clear();
robot_prefix_= "robot_";
scan_topic_postfix_= "/scan";
odom_frame_postfix_= "/odom";
base_frame_postfix_= "/base_footprint";
vel_topic_postfix_= "/mobile_base/commands/velocity";
}
MultiMRS::~MultiMRS()
{
}
주행 Parameters
launch 파일로부터 주행과 관련된 parameters를 불러 들인다.
void MultiMRS::init()
{
// 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 );
}
Subscribers, Publishers 설정
topic 수신, 발신을 위한 subscribers, publishers를 설정한다. 아래 코드를 보면 로봇 개수만큼 vector에 push_back 하는 것을 볼 수 있다. 특히 scan의 경우 하나의 callback 함수에 모든 로봇의 scan topic이 연결되어 있다. callback함수에서 어떤 로봇의 scan topic인지를 구별하기 위해 robot id를 callback 함수의 인자로 전달한다.
/*
* Run and set publishers and subscribers.
*/
void MultiMRS::run()
{
int queue_size= 50;
// subscribe the scan message
for( int i=0; i<NUM_ROBOTS; i++ )
{
std::string id_str= boost::lexical_cast<std::string>(i);
// ROS_WARN( "id: %d, id_str: %s", i, id_str.c_str() );
std::string scan_topic_name= robot_prefix_ + id_str + scan_topic_postfix_;
ROS_WARN( "scan_topic name: %s", scan_topic_name.c_str() );
scan_sub_.push_back( ros_nh_.subscribe<sensor_msgs::LaserScan>( scan_topic_name, queue_size, boost::bind( &MultiMRS::scanCB, this, _1, i ) ) );
std::string velocity_topic_name= robot_prefix_ + id_str + vel_topic_postfix_;
ROS_WARN( "velocity_topic name: %s", velocity_topic_name.c_str() );
vel_pub_.push_back( ros_nh_.advertise<geometry_msgs::Twist>( velocity_topic_name, 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();
}
Scan callback 작성하기
모든 scan topics을 이 함수에서 받아들여서, 인자로 들어온 robot id에 맞게 해당 로봇 객체의 processScan 함수를 호출하여 scan 데이터를 처리한다. 아래 코드에서 Turtlebot과 Kobuki의 scan 데이터 인덱싱이 다르므로, 인덱스 리버스하는 것이 포함되어 있다. 즉, 기존에 Turtlebot용으로 작성한 코드를 수정하지 않고, Kobuki scan 데이터를 reverse하여 로봇에 전달한다. processScan 함수의 결과는 로봇의 주행 속도이므로, 이것을 인자에 포함하여 결과로 받게 되어 있다. 속도값을 각 로봇의 velocity publisher에 넣어 발신한다.
/*
* scan callback
*/
void MultiMRS::scanCB( const sensor_msgs::LaserScanConstPtr& _scan, const int _robot_id )
{
#if 1
ROS_WARN( "scanCB, %d", _robot_id );
#endif
// initialize scan data
if( !got_first_scan_ )
{
// initialize scan
initScan( _scan );
// now we got the first scan
got_first_scan_= true;
return;
}
// filter range readings
ScanRanges ranges;
// reverse the scan for kobuki. @note kobuki scan indices are opposite to the turtlebot scan
// for( int i=0; i<_scan->ranges.size(); i++ )
for( int i=_scan->ranges.size()-1; i>=0; 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
{
// insert the scan
ranges.push_back( _scan->ranges[i] );
}
}
// get odometry pose
geometry_msgs::Pose2D odom_pose;
getOdomPose( _scan->header.stamp, _robot_id, &odom_pose );
#if 0
ROS_INFO( "id: %d, odom x, y, th: %.3f, %.3f, %.3f", _robot_id, odom_pose.x, odom_pose.y, odom_pose.theta );
#endif
// process the scan for the robot and get the commanded velocity
geometry_msgs::Twist velocity;
robots_[_robot_id].processScan( ranges, odom_pose, &velocity );
// publish velocity
vel_pub_[_robot_id].publish( velocity );
}
scan 파라미터 초기화
최초 scan을 수신하여 scan 파라미터를 초기화 한다. scan 데이터가 모두 동일한 형태이므로 모든 로봇에 대해서 딱 한 번만 호출하여 설정한다.
/*
* Initialize the scan data with the fist scan data
* @param scan the first scan data to initialize the map
*/
bool MultiMRS::initScan( const sensor_msgs::LaserScanConstPtr& _scan )
{
#if __DEBUG__
ROS_WARN( "Initializing scan..." );
#endif
// scan parameters
params_.scan_angle_min= _scan->angle_min;
params_.scan_angle_max= _scan->angle_max;
params_.scan_angle_inc= _scan->angle_increment;
ROS_INFO( "Scan angles top down in scan frame: min: %.3f, max: %.3f, inc: %.3f",
params_.scan_angle_min, params_.scan_angle_max, params_.scan_angle_inc );
// set parameters for each robot
for( int i=0; i<NUM_ROBOTS; i++ )
{
robots_[i].setParams( params_ );
}
#if __DEBUG__
ROS_INFO( "Scan initialization completed" );
#endif
// return success
return true;
}
tf2에서 odom pose 추출
tf2에서 odom pose를 얻는 함수이다. 본 강좌에서는 odom pose가 로봇 주행에 사용되지는 않지만, SLAM을 구현하려면 필요하다. 예비적인 차원에서 로봇의 processScan함수에서도 odom pose를 인자로 받게 해 놓았다.
/*
* Get odom pose from the scan pose
* @param stamp ros time of the scan
* @param odom_pose resulting pose
*/
bool MultiMRS::getOdomPose( const ros::Time& _stamp, const int _robot_id, geometry_msgs::Pose2D* _odom_pose )
{
geometry_msgs::TransformStamped odom_msg;
try
{
std::string id_str= boost::lexical_cast<std::string>( _robot_id );
std::string odom_frame= robot_prefix_ + id_str + odom_frame_postfix_;
std::string base_frame= robot_prefix_ + id_str + base_frame_postfix_;
odom_msg= tf2_buffer_->lookupTransform( odom_frame, 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;
}
main 함수 작성하기
MultiMRS 객체를 생성하고, 초기화하고, run 함수를 호출한 후 ros::spin()을 호출하여 ROS가 제어권을 갖고 callback을 처리하게 한다.
/*
* Main
*/
int main( int argc, char** argv )
{
// ros init
ros::init( argc, argv, "multi_mrs_node" );
// create a MRS instance
MultiMRS mrs;
// initialize mrs
mrs.init();
// 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;
}
main launch 파일 작성하기
Gazebo에 kobuki 여러 대 올리기에 사용한 launch 파일을 아래와 같이 수정한다. multi-mrs.launch.xml 파일이 새롭게 추가된다. simple.world 파일이나 mult-robots.launch 파일은 동일하다.
<!-- Launches Kobuki Gazebo simulation in an empty world -->
<launch>
<!-- start Gazebo with an empty 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="$(find multi_robots_sim)/worlds/simple.world"/>
</include>
<!-- include multiple robots -->
<include file="$(find multi_robots_sim)/launch/includes/multi-robots.launch.xml"/>
<!-- multiple mobile robot simulation -->
<include file="$(find multi_robots_sim)/launch/includes/multi-mrs.launch.xml"/>
</launch>
multiple_kobuki.launch 파일 작성하기
MultiMRS 객체 실행 노드와 주행에 필요한 파라미터를 설정한다. 여기서 주의할 것은 Turtlebot 강좌에서의 센서위치와 Kobuki의 센서위치가 다르기 때문에 장애물로 판단되어야 하는 감지 거리 및 Wall Following 거리값을 변경해야 한다. Wall Following이 제대로 되지 않으면 관련 파라미터 값들을 변경한다. 여기에 기재된 값들도 Wall Following이 매끄럽게 되지 않는다. 보다 정교한 Wall Following을 하려면 로봇 주행 클래스 함수와 이곳의 파라미터를 적절히 try-and-error로 수정해야 한다.
<!--
Multi-Robots Simulator
-->
<launch>
<!-- multi mrs -->
<node pkg="multi_robots_sim" type="multi_robots_sim_node" name="multi_robots_sim_node" output="screen" >
<param name="odom_frame" value="odom"/>
<param name="base_frame" value="base_footprint"/>
<param name="max_range" value="3.0"/>
<param name="min_range" value="0.0"/>
<param name="range_noise_ratio" value="0.0"/> <!-- scan range noise depending on the range percent/100@range default: 0.02 -->
<param name="throttle_scans" value="1"/>
<param name="right_section_angle" value="80" /> <!-- right section angle in degrees -->
<param name="front_section_min_angle" value="-40" /> <!-- front section minimum angle in degrees -->
<param name="front_section_max_angle" value="40" /> <!-- front section minimum angle in degrees -->
<param name="left_section_angle" value="-80" /> <!-- left section angle in degrees -->
<param name="side_section_interval" value="10" />
<param name="obs_dist_thresh_r" value="0.6"/>
<param name="obs_dist_thresh_fr" value="0.6"/>
<param name="obs_dist_thresh_fl" value="0.6"/>
<param name="obs_dist_thresh_l" value="0.6"/>
<param name="obs_dist_thresh_emerg" value="0.35"/> <!-- emergent distance threshold -->
<param name="obs_count_thresh_r" value="1"/>
<param name="obs_count_thresh_fr" value="1"/>
<param name="obs_count_thresh_fl" value="1"/>
<param name="obs_count_thresh_l" value="1"/>
<param name="obs_count_thresh_emerg" value="1"/> <!-- emergent count threshold -->
<param name="drive_linvel" value="0.3"/> <!-- driving linear velocity -->
<param name="obs_approach_linvel" value="0.1"/> <!-- approaching linear velocity -->
<param name="obs_avoid_angvel" value="0.3"/> <!-- avoiding angular velocity -->
<param name="desired_wf_dist" value="0.3"/>
<param name="wf_slow_pgain" value="1.0"/>
<param name="wf_slow_dgain" value="0.1"/>
<param name="robot_radius" value="0.12"/>
</node>
</launch>
