Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use odom tf instead of odom topic #27

Open
Maik13579 opened this issue Aug 17, 2023 · 1 comment
Open

Use odom tf instead of odom topic #27

Maik13579 opened this issue Aug 17, 2023 · 1 comment

Comments

@Maik13579
Copy link

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
Copy link

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?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants