目錄
slam_karto是open_karto的ROS的封裝。
1 laserCallback()
直接看雷達消息的回調函數，調用了 addScan（）與 updateMap()。
void
SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
static ros::Time last_map_update(0,0);
// Check whether we know about this laser yet
karto::LaserRangeFinder* laser = getLaser(scan);
if(!laser)
{
ROS_WARN("Failed to create laser device for %s; discarding scan",
scan->header.frame_id.c_str());
return;
}
// addScan()根據當前的雷達數據，返回當前的base_link在odom下的座標 odom_pose
karto::Pose2 odom_pose;
if(addScan(laser, scan, odom_pose))
{
ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
odom_pose.GetX(),
odom_pose.GetY(),
odom_pose.GetHeading());
publishGraphVisualization();
if(!got_map_ ||
(scan->header.stamp - last_map_update) > map_update_interval_)
{
if(updateMap())
{
last_map_update = scan->header.stamp;
got_map_ = true;
ROS_DEBUG("Updated the map");
}
}
}
}
2 updateMap()
updateMap()方法就是更新ros的地圖，每個柵格都需要重新生成。
通過mapper中所有的雷達數據進行karto的佔用柵格地圖的構建，這個是每次雷達數據的到來都會重新生成一遍這個地圖。
之後將karto的柵格地圖轉換成ros的地圖，轉換時分別填入 -1，0, 100 三種數值。
bool
SlamKarto::updateMap()
{
boost::mutex::scoped_lock lock(map_mutex_);
karto::OccupancyGrid* occ_grid =
karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);
if(!occ_grid)
return false;
if(!got_map_) {
map_.map.info.resolution = resolution_;
map_.map.info.origin.position.x = 0.0;
map_.map.info.origin.position.y = 0.0;
map_.map.info.origin.position.z = 0.0;
map_.map.info.origin.orientation.x = 0.0;
map_.map.info.origin.orientation.y = 0.0;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
// Translate to ROS format
kt_int32s width = occ_grid->GetWidth();
kt_int32s height = occ_grid->GetHeight();
karto::Vector2<kt_double> offset = occ_grid->GetCoordinateConverter()->GetOffset();
if(map_.map.info.width != (unsigned int) width ||
map_.map.info.height != (unsigned int) height ||
map_.map.info.origin.position.x != offset.GetX() ||
map_.map.info.origin.position.y != offset.GetY())
{
map_.map.info.origin.position.x = offset.GetX();
map_.map.info.origin.position.y = offset.GetY();
map_.map.info.width = width;
map_.map.info.height = height;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
}
for (kt_int32s y=0; y<height; y++)
{
for (kt_int32s x=0; x<width; x++)
{
// Getting the value at position x,y
kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
switch (value)
{
case karto::GridStates_Unknown:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
break;
case karto::GridStates_Occupied:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
break;
case karto::GridStates_Free:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
break;
default:
ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
break;
}
}
}
// Set the header information on the map
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = map_frame_;
sst_.publish(map_.map);
sstm_.publish(map_.map.info);
delete occ_grid;
return true;
}
3 getOdomPose()
addScan()首先調用了getOdomPose()，獲取當前時刻base_link在odom座標系下的位姿。
bool
SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
{
// Get the robot's pose
// 聲明一個base_link座標系下的(0,0,0)座標，就相當於base_link
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0),
tf::Vector3(0,0,0)), t, base_frame_);
tf::Stamped<tf::Transform> odom_pose;
try
{
// 將ident 從 base_link 座標系 轉換到 odom_frame_ 座標系下，存在odom_pose裏
// 效果就是 odom pose 存儲了base_link在當前地圖上的位置
tf_.transformPose(odom_frame_, ident, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
double yaw = tf::getYaw(odom_pose.getRotation());
karto_pose =
karto::Pose2(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw);
return true;
}
transformPose()，轉換座標的方法，將base_link的座標轉換到odom座標系下，具體函數說明在另一個博客中：
https://blog.csdn.net/tiancailx/article/details/103804070
// Get the robot's pose
// 聲明一個base_link座標系下的(0,0,0)座標，就相當於base_link
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0),
tf::Vector3(0,0,0)), t, base_frame_);
tf::Stamped<tf::Transform> odom_pose;
try
{
// 將ident 從 base_link 座標系 轉換到 odom_frame_ 座標系下，存在odom_pose裏
// 效果就是 odom pose 存儲了base_link在當前地圖上的位置
tf_.transformPose(odom_frame_, ident, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
4 addScan（）
bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)
{
/*
1、對於激光laser的輸入數據scan，添加局部的scan 數據(range_scan)
2、將局部數據range_scan 添加到地圖中:processed = mapper_->Process(range_scan) 以達到矯正位姿的效果；
*/
if(!getOdomPose(karto_pose, scan->header.stamp))
return false;
// Create a vector of doubles for karto
std::vector<kt_double> readings;
if (lasers_inverted_[scan->header.frame_id]) {
for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
it != scan->ranges.rend();
++it)
{
readings.push_back(*it);
}
} else {
for(std::vector<float>::const_iterator it = scan->ranges.begin();
it != scan->ranges.end();
++it)
{
readings.push_back(*it);
}
}
// create localized range scan
karto::LocalizedRangeScan* range_scan =
new karto::LocalizedRangeScan(laser->GetName(), readings);
range_scan->SetOdometricPose(karto_pose); // 輸入里程計的位姿，作爲初始猜測
range_scan->SetCorrectedPose(karto_pose);
// Add the localized range scan to the mapper
bool processed;
if((processed = mapper_->Process(range_scan)))
{
//std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
// Compute the map->odom transform
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform(tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
scan->header.stamp, base_frame_),odom_to_map);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform from base_link to odom failed\n");
odom_to_map.setIdentity();
}
map_to_odom_mutex_.lock();
map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
tf::Point( odom_to_map.getOrigin() ) ).inverse();
map_to_odom_mutex_.unlock();
// Add the localized range scan to the dataset (for memory management)
dataset_->Add(range_scan);
}
else
delete range_scan;
return processed;
}
5 檢查雷達座標系是否是倒着安裝
// To account for lasers that are mounted upside-down,
// we create a point 1m above the laser and transform it into the laser frame
// if the point's z-value is <=0, it is upside-down
// 爲了計算數據顛倒的laser，我們在激光上方1米處創建一個點，並將其轉換爲激光幀，如果點的z值<=0，則它是顛倒的
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
tf::Stamped<tf::Vector3> up(v, scan->header.stamp, base_frame_);
try
{
tf_.transformPoint(scan->header.frame_id, up, up);
ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
}
catch (tf::TransformException& e)
{
ROS_WARN("Unable to determine orientation of laser: %s", e.what());
return NULL;
}
bool inverse = lasers_inverted_[scan->header.frame_id] = up.z() <= 0;
if (inverse)
ROS_INFO("laser is mounted upside-down");
REFERENCES
kartoSLAM代碼解讀 ： https://www.e-learn.cn/topic/3175753