How to store last message from publisher [closed]

edit

Hi there,

I was hoping if I could get some help regarding subscriber and callback function as I have multiple questions.

1)Specifically, I would like to subscribe the first message of my topic and send it to the first pointer(cloud_in) and then subscribe the very next message of the topic and send it to another pointer(cloud2_in). How do I make sure that the message is going to the respective pointer? I am posting my code for reference here.

2) I have a sensor_msgs/PointCloud2 topic that needs to be added to another sensor_msgs/PointCloud2 topic after each rosspin. This is shown by the line "Final+=*cloud_in" where I am adding the pointers together and then converting Final back to sensor_msgs/PointCloud2 topic "output". However whenever I visualize the output topic, I see that the topics are added for the current two messages but does not retain the old value of output. Is there a way to store the previous value of output?

I would appreciate if I could get some guidance here. Apologies if this sounds trivial, I'm a novice in C++ and ROS. I am using ROS Hydro on Ubuntu 12.04.Thank you.

ros::Publisher _pub;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGB>);

    void 
    cloud_cb2 (const sensor_msgs::PointCloud2ConstPtr& next_input)
    {
      pcl::fromROSMsg (*next_input, *cloud_in);
      //remove NAN points from the cloud 
      std::vector<int> indices; 
      pcl::removeNaNFromPointCloud(*cloud_in,*cloud_in, indices); 
    // Convert the sensor_msgs/PointCloud2 data to pcl::PointCloud
      pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2_in (new pcl::PointCloud<pcl::PointXYZRGB>);
      pcl::fromROSMsg (*next_input, *cloud2_in);
      //remove NAN points from the cloud 
      std::vector<int> indices2; 
      pcl::removeNaNFromPointCloud(*cloud2_in,*cloud2_in, indices2); 

      pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
      icp.setInputSource(cloud2_in);
      icp.setInputTarget(cloud_in);
      pcl::PointCloud<pcl::PointXYZRGB> Final;
      icp.align(Final);
      Final+=*cloud_in;

      sensor_msgs::PointCloud2 output;
      pcl::toROSMsg( Final, output );
      _pub.publish(output);
    }

    int main (int argc, char** argv) 
    {
      ros::init (argc, argv, "my_pcl_tutorial");
      ros::NodeHandle nh;

      // ROS subscriber for /camera/depth_registered/points
      ros::Subscriber sub = nh.subscribe(
                        "/Camera1/depth_registered/points",
                        2,
                        cloud_cb2
                        );

      // Create ROS publisher for transformed pointcloud
      _pub = nh.advertise<sensor_msgs::PointCloud2>(
                               "output",
                               1
                               );
      // Spin
      ros::spin ();
    }