Unable to convert pcl data
I am trying to convert pcl data type to ROS message type using command pcl::toROSMsg and publish two topics. However, I cannot receive any tf info and the error shows here:
terminate called after throwing an instance of 'tf2::LookupException'
what(): "map" passed to lookupTransform argument target_frame does not exist.
Below is my code:
#include <iostream>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/registration/icp.h>
#include <Eigen/Dense>
int main(int argc, char **argv)
{
//Initialize ROS
ros::init(argc, argv, "pcl");
ros::NodeHandle nh;
ros::Publisher map_pub;
ros::Publisher scene_pub;
tf::TransformListener listener;
tf::StampedTransform transform;
tf::Quaternion q;
static tf::TransformBroadcaster br;
//Create a ROS publisher for the output point cloud
map_pub = nh.advertise<sensor_msgs::PointCloud2>("/map", 100);
scene_pub = nh.advertise<sensor_msgs::PointCloud2>("/scene", 100);
//Load the file
const pcl::PointCloud<pcl::PointXYZ>::Ptr map (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile ("/home/parallels/Downloads/target.pcd", *map);
const pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile ("/home/parallels/Downloads/scene.pcd", *scene);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//Set the input source and target
icp.setInputSource(scene);
icp.setInputTarget(map);
//Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance(0.05);
//Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations(5000);
//Set the transformation epsilon (criterion 2)
icp.setTransformationEpsilon(1e-8);
//Set the euclidean distance difference epsilon (criterion 3)
icp.setEuclideanFitnessEpsilon(1);
//Perform the alignment
pcl::PointCloud<pcl::PointXYZ> scene_cloud_registered;
icp.align(scene_cloud_registered);
//Obtain the transformation that aligned scene_cloud to scene_cloud_registered
Eigen::Matrix4f transformation = icp.getFinalTransformation();
std::cout << "Transformation matrix : " << std::endl << transformation;
********************Below is where I believe something goes wrong**********************
//Convert pcd data type to ROS message type
sensor_msgs::PointCloud2 map_cloud;
sensor_msgs::PointCloud2 scene_cloud;
pcl::toROSMsg(*map, map_cloud);
pcl::toROSMsg(*scene, scene_cloud);
//Publish the point cloud message and tf
map_pub.publish(map_cloud);
scene_pub.publish(scene_cloud);
//Calculate tf between /map and /scene
listener.lookupTransform("/map", "/scene", ros::Time(0), transform);
br.sendTransform(transform);
//Spin
ros::spin();
}
I assume I did something wrong in the step of converting pcl data to ROS message type since I can obtain transformation matrix. I have searched through the Internet but still could not fix this problem. Can someone give me some hint? Thank you guys so much!
Can you post a picture of your TF tree so we know these frames actually exist?
Or cut and paste the output of
rosrun tf tf_echo map scene
for mostly the same information in text form.Hi @stevejp and @lucasw, thank you for your response and sorry for my late response. Yes I did not create those frames since I thought that topic name was also the frame name.