rsasaki0109 / lidar_localization_ros2

3D LIDAR Localization using NDT/GICP and pointcloud map in ROS 2 (Not SLAM)
BSD 2-Clause "Simplified" License
285 stars 55 forks source link

Use odom tf instead of odom topic #27

Open Maik13579 opened 1 year ago

Maik13579 commented 1 year ago

Hey, Instead of subscribing to an odometry topic, you could just transform the lidar point cloud to the odom frame.

This would allow your localization package to fix the error that odometry makes. Of course, the path you publish doesn't make sense anymore because it doesn't take odometry into account. But the combined transformation (map->odom->base_link) would be a more accurate pose of the robot.

I tested this with kiss-icp (a lidar odometry pipeline) and got a really good localization out of it.

This is the change I made to your code to make it work.

So in your cloudReceived callback you could add the transform after you filter out points that are too close or too far.

double r;
  pcl::PointCloud<pcl::PointXYZI> tmp;
  for (const auto & p : filtered_cloud_ptr->points) {
    r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0));
    if (scan_min_range_ < r && r < scan_max_range_) {
      tmp.push_back(p);
    }
  }
  // Transform the filtered and pruned point cloud into the odom frame.
  sensor_msgs::msg::PointCloud2 pruned_cloud_ros;
  pcl::toROSMsg(tmp, pruned_cloud_ros);
  sensor_msgs::msg::PointCloud2 transformed_cloud;
  try {
    // The lookupTransform arguments are target frame, source frame, and time.
    pruned_cloud_ros.header.frame_id = msg->header.frame_id; //Set the header of the pointcloud to the original frame
    tfbuffer_.transform(pruned_cloud_ros, transformed_cloud, "odom", tf2::durationFromSec(0.1)); //TODO get odom frame from params
  } catch (tf2::TransformException &ex) {
    RCLCPP_WARN(get_logger(), "%s", ex.what());
    return;
  }

  pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>());
  pcl::fromROSMsg(transformed_cloud, *transformed_cloud_ptr);

Additionally I had to remove the static odom->velodyne frame from you launchfile.

lidarmansiwon commented 11 months ago

I have a problem to build code. error contain this message error: ‘tfbuffer’ was not declared in this scope 380 | tfbuffer.transform(pruned_cloud_ros, transformed_cloud, "odom", tf2::durationFromSec(0.1)); //TODO get odom frame from params

how can i fix it?