bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)
{
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;
}
karto::LaserRangeFinder*
SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)
{
// Check whether we know about this laser yet
if(lasers_.find(scan->header.frame_id) == lasers_.end())
{
// New laser; need to create a Karto device for it.
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = scan->header.frame_id;
ident.stamp_ = scan->header.stamp;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return NULL;
}
double yaw = tf::getYaw(laser_pose.getRotation());
ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f",
scan->header.frame_id.c_str(),
laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw);
// 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
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");
// Create a laser range finder device and copy in data from the first
// scan
std::string name = scan->header.frame_id;
karto::LaserRangeFinder* laser =
karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));
laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw));
laser->SetMinimumRange(scan->range_min);
laser->SetMaximumRange(scan->range_max);
laser->SetMinimumAngle(scan->angle_min);
laser->SetMaximumAngle(scan->angle_max);
laser->SetAngularResolution(scan->angle_increment);
// TODO: expose this, and many other parameters
//laser_->SetRangeThreshold(12.0);
// Store this laser device for later
lasers_[scan->header.frame_id] = laser;
// Add it to the dataset, which seems to be necessary
dataset_->Add(laser);
}
return lasers_[scan->header.frame_id];
}
karto::LaserRangeFinder*
SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)
{
// Check whether we know about this laser yet
if(lasers_.find(scan->header.frame_id) == lasers_.end())
{
// New laser; need to create a Karto device for it.
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = scan->header.frame_id;
ident.stamp_ = scan->header.stamp;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return NULL;
}
double yaw = tf::getYaw(laser_pose.getRotation());
ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f",
scan->header.frame_id.c_str(),
laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw);
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
tf::Quaternion q;
q.setRPY(0.0, 0.0, scan->angle_min);
tf::Stamped<tf::Quaternion> min_q(q, scan->header.stamp,
scan->header.frame_id);
q.setRPY(0.0, 0.0, scan->angle_max);
tf::Stamped<tf::Quaternion> max_q(q, scan->header.stamp,
scan->header.frame_id);
try
{
tf_.transformQuaternion(base_frame_, min_q, min_q);
tf_.transformQuaternion(base_frame_, max_q, max_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return false;
}
double angle_min = tf::getYaw(min_q);
double angle_max = tf::getYaw(max_q);
bool inverse = lasers_inverted_[scan->header.frame_id] = angle_max < angle_min;
if (inverse)
ROS_INFO("laser is mounted upside-down");
// Create a laser range finder device and copy in data from the first
// scan
std::string name = scan->header.frame_id;
karto::LaserRangeFinder* laser =
karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));
laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw));
laser->SetMinimumRange(scan->range_min);
laser->SetMaximumRange(scan->range_max);
laser->SetMinimumAngle(scan->angle_min);
laser->SetMaximumAngle(scan->angle_max);
laser->SetAngularResolution(scan->angle_increment);
// TODO: expose this, and many other parameters
//laser_->SetRangeThreshold(12.0);
// Store this laser device for later
lasers_[scan->header.frame_id] = laser;
// Add it to the dataset, which seems to be necessary
dataset_->Add(laser);
}
return lasers_[scan->header.frame_id];
}
/**
* @brief SlamKarto::addScan
* @param laser LaserRangeFinder ref base
* @param scan 传感器数据 ref laser
* @param karto_pose odom_pose 回调
* @return
*/
bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)
{
if(!getOdomPose(karto_pose, scan->header.stamp)) // 将odom与base保持坐标系对齐． 返回 odom --> karto_pose
return false;
// Create a vector of doubles for karto
std::vector<kt_double> readings;
//readings 原始scan laser -> reading
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); //laser range messages
}
}
else
{
for(std::vector<float>::const_iterator it = scan->ranges.begin();
it != scan->ranges.end();
++it)
{
readings.push_back(*it);
debugPrint_<<" "<< *it;
}
}
debugPrint_<<" " <<endl;
if(debug_flag_)
debugPrint_<<"reading.size() "<<readings.size()<<endl;
// create localized range scan 创建rangeScan
karto::LocalizedRangeScan* range_scan =
new karto::LocalizedRangeScan(laser->GetName(), readings);//
range_scan->SetOdometricPose(karto_pose);// Sets the odometric pose of this scan
range_scan->SetCorrectedPose(karto_pose);// Moves the scan by moving the robot pose to the given location.
if(debug_print_flag_)
debugPrint_<<"1.1 addScan-karto_pose-: "<<karto_pose.GetX()<<" "<<karto_pose.GetY()<<" "<< karto_pose.GetHeading() <<" "<<endl;
// Add the localized range scan to the mapper
bool processed; //range_scan contain reading and predict pose of robot
if((processed = mapper_->Process(range_scan))) // pose 为base相对odom的pose
{
//std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
if(debug_print_flag_)
debugPrint_<<"1.2 addScan-corrected_pose-: "<<corrected_pose.GetX()<<" "<<corrected_pose.GetY()<<" "<< corrected_pose.GetHeading() <<" "<<endl;
// 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();
//debugPrint_<< " x y z "<< odom_to_map.getOrigin().getX() <<" "<< odom_to_map.getOrigin().getY()<<" "<<odom_to_map.getOrigin().getZ()<<endl;
if(debug_print_flag_)
debugPrint_<<"1.3 addScan-odom_to_map: "<<odom_to_map.getOrigin().getX() <<" "<< odom_to_map.getOrigin().getY()<<" "<<endl;
// Add the localized range scan to the dataset (for memory management)
dataset_->Add(range_scan);
}
else
delete range_scan;
return processed;
}
