Performing ICP in C++ with two .bag files

edit

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++