Skip to content

Commit

Permalink
Reorganize scanCallback and add comments (tu-darmstadt-ros-pkg#89)
Browse files Browse the repository at this point in the history 
* Refactor, reformat and comment scanCallback

* make rosPointCloudToDataContainer void

* make rosLaserScanToDataContainer void
  • Loading branch information
@marcelino-pensa
marcelino-pensa committed Mar 20, 2021
1 parent d1c7f50 commit 9111a90
Show file tree
Hide file tree
Showing 2 changed files with 79 additions and 66 deletions.
141 changes: 77 additions & 64 deletions hector_mapping/src/HectorMappingRos.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -233,7 +233,8 @@ HectorMappingRos::~HectorMappingRos()

void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
{
if (pause_scan_processing_) {
if (pause_scan_processing_)
{
return;
}

Expand All @@ -242,93 +243,107 @@ void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
hectorDrawings->setTime(scan.header.stamp);
}

ros::WallTime startTime = ros::WallTime::now();

ros::WallTime start_time = ros::WallTime::now();
if (!p_use_tf_scan_transformation_)
{
if (rosLaserScanToDataContainer(scan, laserScanContainer,slamProcessor->getScaleToMap()))
{
slamProcessor->update(laserScanContainer,slamProcessor->getLastScanMatchPose());
}
// If we are not using the tf tree to find the transform between the base frame and laser frame,
// then just convert the laser scan to our data container and process the update based on our last
// pose estimate
this->rosLaserScanToDataContainer(scan, laserScanContainer, slamProcessor->getScaleToMap());
slamProcessor->update(laserScanContainer, slamProcessor->getLastScanMatchPose());
}
else
{
ros::Duration dur (0.5);
// If we are using the tf tree to find the transform between the base frame and laser frame,
// let's get that transform
const ros::Duration dur(0.5);
tf::StampedTransform laser_transform;
if (tf_.waitForTransform(p_base_frame_, scan.header.frame_id, scan.header.stamp, dur))
{
tf_.lookupTransform(p_base_frame_, scan.header.frame_id, scan.header.stamp, laser_transform);
}
else
{
ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.", p_base_frame_.c_str(), scan.header.frame_id.c_str());
return;
}

if (tf_.waitForTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp,dur))
// Convert the laser scan to point cloud
projector_.projectLaser(scan, laser_point_cloud_, 30.0);

// Publish the point cloud if there are any subscribers
if (scan_point_cloud_publisher_.getNumSubscribers() > 0)
{
tf::StampedTransform laserTransform;
tf_.lookupTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp, laserTransform);
scan_point_cloud_publisher_.publish(laser_point_cloud_);
}

//projector_.transformLaserScanToPointCloud(p_base_frame_ ,scan, pointCloud,tf_);
projector_.projectLaser(scan, laser_point_cloud_,30.0);
// Convert the point cloud to our data container
this->rosPointCloudToDataContainer(laser_point_cloud_, laser_transform, laserScanContainer, slamProcessor->getScaleToMap());

if (scan_point_cloud_publisher_.getNumSubscribers() > 0){
scan_point_cloud_publisher_.publish(laser_point_cloud_);
}
// Now let's choose the initial pose estimate for our slam process update
Eigen::Vector3f start_estimate(Eigen::Vector3f::Zero());
if (initial_pose_set_)
{
// User has requested a pose reset
initial_pose_set_ = false;
start_estimate = initial_pose_;
}
else if (p_use_tf_pose_start_estimate_)
{
// Initial pose estimate comes from the tf tree
try
{
tf::StampedTransform stamped_pose;

Eigen::Vector3f startEstimate(Eigen::Vector3f::Zero());
tf_.waitForTransform(p_map_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));
tf_.lookupTransform(p_map_frame_, p_base_frame_, scan.header.stamp, stamped_pose);

if(rosPointCloudToDataContainer(laser_point_cloud_, laserTransform, laserScanContainer, slamProcessor->getScaleToMap()))
const double yaw = tf::getYaw(stamped_pose.getRotation());
start_estimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(), stamped_pose.getOrigin().getY(), yaw);
}
catch(tf::TransformException e)
{
if (initial_pose_set_){
initial_pose_set_ = false;
startEstimate = initial_pose_;
}else if (p_use_tf_pose_start_estimate_){

try
{
tf::StampedTransform stamped_pose;

tf_.waitForTransform(p_map_frame_,p_base_frame_, scan.header.stamp, ros::Duration(0.5));
tf_.lookupTransform(p_map_frame_, p_base_frame_, scan.header.stamp, stamped_pose);

tfScalar yaw, pitch, roll;
stamped_pose.getBasis().getEulerYPR(yaw, pitch, roll);

startEstimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(),stamped_pose.getOrigin().getY(), yaw);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str());
startEstimate = slamProcessor->getLastScanMatchPose();
}

}else{
startEstimate = slamProcessor->getLastScanMatchPose();
}


if (p_map_with_known_poses_){
slamProcessor->update(laserScanContainer, startEstimate, true);
}else{
slamProcessor->update(laserScanContainer, startEstimate);
}
ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str());
start_estimate = slamProcessor->getLastScanMatchPose();
}
}
else
{
// If none of the above, the initial pose is simply the last estimated pose
start_estimate = slamProcessor->getLastScanMatchPose();
}

}else{
ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.", p_base_frame_.c_str(), scan.header.frame_id.c_str());
return;
// If "p_map_with_known_poses_" is enabled, we assume that start_estimate is precise and doesn't need to be refined
if (p_map_with_known_poses_)
{
slamProcessor->update(laserScanContainer, start_estimate, true);
}
else
{
slamProcessor->update(laserScanContainer, start_estimate);
}
}

// If the debug flag "p_timing_output_" is enabled, print how long this last iteration took
if (p_timing_output_)
{
ros::WallDuration duration = ros::WallTime::now() - startTime;
ros::WallDuration duration = ros::WallTime::now() - start_time;
ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec()*1000.0f );
}

//If we're just building a map with known poses, we're finished now. Code below this point publishes the localization results.
// If we're just building a map with known poses, we're finished now. Code below this point publishes the localization results.
if (p_map_with_known_poses_)
{
return;
}

poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(), scan.header.stamp, p_map_frame_);

// Publish pose with and without covariances
poseUpdatePublisher_.publish(poseInfoContainer_.getPoseWithCovarianceStamped());
posePublisher_.publish(poseInfoContainer_.getPoseStamped());

// Publish odometry if enabled
if(p_pub_odometry_)
{
nav_msgs::Odometry tmp;
Expand All @@ -339,10 +354,10 @@ void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
odometryPublisher_.publish(tmp);
}

// Publish the map->odom transform if enabled
if (p_pub_map_odom_transform_)
{
tf::StampedTransform odom_to_base;

try
{
tf_.waitForTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));
Expand All @@ -357,7 +372,9 @@ void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_));
}

if (p_pub_map_scanmatch_transform_){
// Publish the transform from map to estimated pose (if enabled)
if (p_pub_map_scanmatch_transform_)
{
tfB_->sendTransform( tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));
}
}
Expand Down Expand Up @@ -453,7 +470,7 @@ void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hec
mapPublisher.mapPublisher_.publish(map_.map);
}

bool HectorMappingRos::rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap)
void HectorMappingRos::rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap)
{
size_t size = scan.ranges.size();

Expand All @@ -477,11 +494,9 @@ bool HectorMappingRos::rosLaserScanToDataContainer(const sensor_msgs::LaserScan&

angle += scan.angle_increment;
}

return true;
}

bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap)
void HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap)
{
size_t size = pointCloud.points.size();
//ROS_INFO("size: %d", size);
Expand Down Expand Up @@ -514,8 +529,6 @@ bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointClou
}
}
}

return true;
}

void HectorMappingRos::setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap)
Expand Down
4 changes: 2 additions & 2 deletions hector_mapping/src/HectorMappingRos.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,8 +83,8 @@ class HectorMappingRos

void publishMap(MapPublisherContainer& map_, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex = 0);

bool rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap);
bool rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap);
void rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap);
void rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap);

void setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap);

Expand Down

0 comments on commit 9111a90

Please sign in to comment.