Question

Hey there,

I have a problem with ROS. I want to perform the Iterative Closest Point Algorithm between to point clouds. Therefore I wrote the following code:

ros::Publisher pub;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in;

//Festlegung der fixed_frame_msg
void fixed_frame_cb (const sensor_msgs::PointCloud2ConstPtr& pc2_msg)
{

  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg ( *pc2_msg, *cloud_in );


}

//Festlegung des camera_frame_msg
void camera_frame_cb (const sensor_msgs::PointCloud2ConstPtr& next_pc2_msg)
{

  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2_in(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg ( *next_pc2_msg, *cloud2_in );


  // Perform ICP
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputSource(cloud2_in);
  icp.setInputTarget(cloud_in);
  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);

  // Convert the pcl/PointCloud to sensor_msgs/PointCloud2
  sensor_msgs::PointCloud2 output;
  pcl::toROSMsg( *cloud2_in, output );

  // Publish the results
  pub.publish( output );

}



int main (int argc, char** argv) {

  // Initialize ROS
  ros::init (argc, argv, "icp");
  ros::NodeHandle n( "~" );

  // Create ROS subscriber for fixed_frame topic
  ros::Subscriber sub = n.subscribe("input/raw",1,fixed_frame_cb);

  // Create ROS subscriber for camera_frame topic
  ros::Subscriber sub2 = n.subscribe("input/raw",1,camera_frame_cb);

  // Create ROS publisher for transformed pointcloud
  pub = n.advertise<sensor_msgs::PointCloud2>("transformed_point_cloud",1);

  // Spin
  ros::spin ();
}

Now I want to play the two bag-files with the following command:

rosbag play -l 2017-06-07-13-49-48.bag 2017-06-07-14-05-40.bag /camera/depth/points:=icp/input/raw

If I want to display the result in RViz, the display remains empty.

Does anyone know the problem and can help me? I am new to ROS and C++

Asked by jrgen on 2017-09-19 02:49:29 UTC

Answer 1

Have you checked if the topic is published via

rostopic hz your_topic_name.

Also make sure that the fields are not empty (Data part of the message).

I am asking this because there is a possibility that your topic is publishing but it isn't displayed in rviz. You are missing the 'header' part of the message which is quite crucial for displaying on rviz.

After the lines:

// Convert the pcl/PointCloud to sensor_msgs/PointCloud2
sensor_msgs::PointCloud2 output;
pcl::toROSMsg( *cloud2_in, output );

add the lines:

output.header.stamp = ros::Time::now();
output.header.frame_id = "fixed_frame"; // You can change this for your needs

and setting the fixed_frame in rviz to fixed_frame have to display your message if your message isn't empty.

Another possible issue might arise due to cloud_in container being empty. This can happen if camera_frame_cb is called before fixed_frame_cb(You are filling the container at the fixed_frame_cb). It might be a good idea to check the size of the data in the cloud_in container, or just keep a boolean to indicate that it is initialized.

Just a final note, you are converting your input to output at the lines below, I am guessing you want to publish aligned pointCloud.

pcl::toROSMsg( *cloud2_in, output );

Asked by ulashmetalcrush on 2020-12-29 08:09:38 UTC

Performing ICP in C++ with two .bag files

Comments

Answers

Comments