토픽 결과물: 로봇이 어디를 돌아다녔는지 알 수 있게되요.
로봇이 주행한 면적을 Grid Map으로 표시합니다. Coverage 맵은 Floorplan 면적 대비 cover한 영역 계산에 이용 될 수 있습니다.
설명 동영상
include files 추가
mobile_robot_sim 패키지의 src 디렉터리의 mobile_robot_sim.cpp 파일에 boot thread와 grid map 메시지 관련 파일을 추가합니다.
// boost thread
#include <boost/thread.hpp>
// coverage grid map
#include <nav_msgs/OccupancyGrid.h>
struct, enum 추가
coverage map관련 enum과 parameters를 추가한다. map frame을 추가하고, map size, resolution 파라미터를 추가합니다.
enum
{
GRID_UNKNOWN= -1,
GRID_EMPTY= 0,
GRID_OCCUPIED= 100
};
struct MRSParameters
{
...
/// map frame
std::string map_frame;
/// map grid resolution
double map_resolution;
/// map minimum size in x axis
double map_xmin;
/// map minimum size in y axis
double map_ymin;
/// map maximum size in x axis
double map_xmax;
/// map maximum size in y axis
double map_ymax;
/// time interval to update the coverage map
double map_update_interval;
/// radius of robot baseline
double robot_radius;
};
MRS 클래스에 멤버 추가
coverage map 관련 멤버 변수와 함수를 추가합니다. map_frame은 현재 SLAM 부분이 구현되어 있지 않기 때문에 odom_frame과 같은 frame으로 설정합니다. map frame이 rviz의 fixed frame으로 사용될 것입니다.
class MRS
{
...
public:
/////// coverage map
/// publish tf; from map frame to odom frame
void publishTransform();
/// update the coverage grid map at the current robot pose
void updateCoverageMap( const geometry_msgs::Pose2D& _pose );
private:
/////// coverage
/// coverage map publisher
ros::Publisher coverage_map_pub_;
/// coverage grid map
nav_msgs::OccupancyGrid coverage_map_;
/// flag for having the coverage map
bool got_map_;
/// mutex for coverage map
boost::mutex map_mutex_;
/// boost thread for tf publish
boost::thread* publish_thread_;
};
init() 함수 수정
map frame과 coverage 맵관련 파라미터를 초기화 합니다. 또한, coverage를 표시하기 위한 robot 반지름 파라미터도 추가합니다.
void MRS::init( const geometry_msgs::Pose2D& _init_pose )
{
...
ros_nh.param<std::string>( "map_frame", params_.map_frame, "map" );
ros_nh.param<double>( "map_xmin", params_.map_xmin, -10.0 );
ros_nh.param<double>( "map_ymin", params_.map_ymin, -10.0 );
ros_nh.param<double>( "map_xmax", params_.map_xmax, 10.0 );
ros_nh.param<double>( "map_ymax", params_.map_ymax, 10.0 );
ros_nh.param<double>( "map_resolution", params_.map_resolution, 0.05 );
ros_nh.param<double>( "robot_radius", params_.robot_radius, 0.12 );
}
run() 함수 수정
map frame과 odom frame간의 관계를 나타내는 transform을 publish하기 위해 boost thread를 사용합니다. 그리고 coverage map을 메시지 형태로 출력하기 위한 publisher도 생성합니다.
void MRS::run()
{
...
// run the publishing thread; 30Hz
publish_thread_= new boost::thread( boost::bind( &MRS::publishTransform, this ) );
// coverage map publisher
coverage_map_pub_= ros_nh_.advertise<nav_msgs::OccupancyGrid>( "coverage_map", 1, true );
}
initScan() 함수 수정
입력받은 맵 관련 파라미터값으로 coverage map을 초기화 합니다. 여기서 coverage map의 origin 설정에 눈여겨 봅니다. 로봇이 놓이 방향에 따라 로봇은 맵의 음의 영역으로 갈 수 있기 때문에, 맵의 origin을 (0, 0, 0)으로 하지 않고, 맵의 최소 x, 최소 y로 설정합니다. 여기서는 map_xmin= -10m, map_ymin= -10m로 설정하였습니다.
bool MRS::initScan( const sensor_msgs::LaserScanConstPtr& _scan )
{
...
// initialize coverage grid map
coverage_map_.header.frame_id= params_.map_frame;
coverage_map_.info.resolution= params_.map_resolution;
coverage_map_.info.height= int( (params_.map_ymax-params_.map_ymin)/params_.map_resolution );
coverage_map_.info.width= int( (params_.map_xmax-params_.map_xmin)/params_.map_resolution );
coverage_map_.info.origin.position.x= params_.map_xmin;
coverage_map_.info.origin.position.y= params_.map_ymin;
coverage_map_.info.origin.position.z= 0.0f;
tf2::Quaternion q;
q.setRPY( 0.0, 0.0, 0.0 );
coverage_map_.info.origin.orientation.x= q.x();
coverage_map_.info.origin.orientation.y= q.y();
coverage_map_.info.origin.orientation.z= q.z();
coverage_map_.info.origin.orientation.w= q.w();
coverage_map_.data.resize( coverage_map_.info.height * coverage_map_.info.width );
}
scanCB() 함수 수정
먼저 함수 초반부에 아래와 같이 map update 관련 변수를 추가합니다. 그리고 후반부에 아래와 같이 coverage map을 update하는 부분과 맵 메시지를 publish하는 부분을 추가합니다.
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;
// time when the map is updated lastly
static ros::Time map_updated_time(0,0);
// map update interval threshold
ros::Duration map_update_interval;
map_update_interval.fromSec( params_.map_update_interval );
...
// publish map in case that either no map generated or update time reached
if( !got_map_ || (_scan->header.stamp - map_updated_time ) > map_update_interval )
{
// update the coverage map
updateCoverageMap( gt_pose_ );
// publish the coverage map message
coverage_map_pub_.publish( coverage_map_ );
// update the time of map updating
map_updated_time= _scan->header.stamp;
}
} // end of scanCB
tf publish 함수 생성
아래와 같이 odom frame과 동일하게 map frame을 생성하고, map frame과 odom frame의 관계를 publish 합니다. 나중에 SLAM 부분이 구현되면, SLAM 결과로 map frame과 odom frame이 달라지게 되며, map frame으로 로봇의 위치를 표시하게 됩니다.
/*
* Publish transform: odom_frame based on the map frame
* @param period publishing period
*/
void MRS::publishTransform()
{
// publish rate 30hz
ros::Rate rate( 1.0/0.033 );
// loop
while( ros::ok() )
{
// set identity transform for now
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp= ros::Time::now();
transformStamped.header.frame_id= params_.map_frame;
transformStamped.child_frame_id= params_.odom_frame;
transformStamped.transform.translation.x= 0.0;
transformStamped.transform.translation.y= 0.0;
transformStamped.transform.translation.z= 0.0;
tf2::Quaternion q;
q.setRPY(0.0, 0.0, 0.0);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
// send the tf
tf2_broadcaster_->sendTransform( transformStamped );
// sleep for other threads
rate.sleep();
}
} // end of publishTransform()
coverage map update 함수 생성
아래와 같이 로봇이 지나간 영역을 coverage map에 새기는 부분을 구현합니다. 편의상 로봇의 형태를 정사각형으로 단순화 하여 계산하였습니다.
/*
* Update the coverage map.
* Set occupancy grids within the robot baseline
* @param pose ground truth pose of the robot
*/
void MRS::updateCoverageMap( const geometry_msgs::Pose2D& _pose )
{
// indices of grid map boundary
int xmin_idx= 0;
int ymin_idx= 0;
int xmax_idx= coverage_map_.info.width;
int ymax_idx= coverage_map_.info.height;
int xmid_idx= int( coverage_map_.info.width/2 );
int ymid_idx= int( coverage_map_.info.height/2 );
int left_idx= (_pose.x - params_.robot_radius)/params_.map_resolution + xmid_idx;
int right_idx= (_pose.x + params_.robot_radius)/params_.map_resolution + xmid_idx;
int top_idx= (_pose.y - params_.robot_radius)/params_.map_resolution + ymid_idx;
int bottom_idx= (_pose.y + params_.robot_radius)/params_.map_resolution + ymid_idx;
// lock the map message
boost::mutex::scoped_lock lock( map_mutex_ );
// loop over the grids occupied by the robot
for( int y=top_idx; y<bottom_idx; y++ )
{
for( int x=left_idx; x<right_idx; x++ )
{
// boundary check
if( x < xmin_idx || x >= xmax_idx || y < ymin_idx || y >= ymax_idx )
continue;
coverage_map_.data[ y*coverage_map_.info.width + x ]= GRID_OCCUPIED;
}
}
// set stamp
coverage_map_.header.stamp= ros::Time::now();
// now we got the map
if( !got_map_ )
got_map_= true;
}
mrs.launch.xml 파라미터 추가
아래와 같이 맵 관련 파라미터를 mrs.launch.xml 파일에 추가합니다.
<!--
mobile robot simulation node and parameters
-->
<launch>
...
<param name="map_frame" value="map"/>
<param name="map_xmin" value="-10.0"/>
<param name="map_xmax" value="10.0"/>
<param name="map_ymin" value="-10.0"/>
<param name="map_ymax" value="10.0"/>
<param name="map_resolution" value="0.05"/>
<param name="robot_radius" value="0.12"/>
</node>
</launch>
노드 실행
노드를 실행하기전에 catkin_make하여 소스를 빌드합니다. 아래와 같이 별도의 terminal에 turtlebot_hokuyo.launch와 rviz.launch를 실행합니다.
# terminal 1
$ roslaunch mobile_robot_sim turtlebot_hokuyo.launch
# terminal 2
$ roslaunch mobile_robot_sim rviz.launch
Gazebo와 RVIZ가 실행되면 아래와 같은 화면이 나타납니다. 로봇이 지나간 영역이 검은색으로 나타납니다
아래 그림과 같이 RVIZ에서 Map 타입으로 "/coverage_map" topic를 수신합니다. 이 때, fixed frame을 map으로 선택해야 합니다. 또한 다음 토픽에서 coverage ratio를 계산하기 위해, Display 메뉴에서 Grid를 off 시킵니다(uncheck). 또한, coverage 계산을 위해 불필요한 로봇 Axes도 off 시켜 아래와 같이 floorplan과 coverage 영역(검은색)만 나타나게 합니다. 그런 후에 coverage map과 floorplan map을 화면 캡쳐하여 이미지를 저장합니다. 저장한 이미지는 다음 토픽에서 영상처리로 coverage ratio를 계산하는데 이용됩니다.
참고로, 위와 같이 RVIZ 환경을 만들어 놓고 RVIZ 메뉴의 File -> Save Config As 를 선택하여 config를 저장해 놓고 쓰면 유용합니다.
