Get 10 values of topic I'm subscribing to
Hello everybody,
I'm trying to create a loop to get the average of special informations of a pointcloud.
void
GetParameters::cloud_CB_Parameter_calc(const sensor_msgs::PointCloud2ConstPtr& cloud_msgs){
ROS_DEBUG("Received input cloud")
try{
ROS_INFO("Cloud-Callback in progress ...");
// lastDynamicUpdate_ = cloud_msgs->header.stamp;
// transform cloud_msgs to worldframe
// ------------------------------------
tfListener_.waitForTransform(strFixedFrame_, cloud_msgs->header.frame_id, cloud_msgs->header.stamp, tfTimeout_);
sensor_msgs::PointCloud2 ff_cloud;
pcl_ros::transformPointCloud(strRobotFrame_, *cloud_msgs, ff_cloud, tfListener_);
// transform ff_cloud (worldframe) to PointXYZ-Cloud temp_cloud
// -------------------------------------------------------------
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(ff_cloud,pcl_pc2);
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);
[... here i calclulate the roll/pitch values for each momental pointcloud ...]
// add the parameter value for each i to a history-vector
// -------------------------------------------------------
history_pitch.push_back(pitch_degree);
history_roll.push_back(roll_degree);
history_zdistance.push_back(d_z);
}
calc_and_publish_Parameters(); // here I want to calculate the average of the history vectors
}
In the following parts of the code I process the data of this pointcloud. I want to get the average of the pitch/and roll angles of the ground plane I get out of this PointCloud.
In another get_parameters_node.cpp I call a main-function and do ros::spin() in it. So I get the values in a open loop. but my goal is to run the node as an function that returns only one value of roll/ pitch
I already tried to build a for-loop around the call-back method, but that didn't worked, like i got a time delay to the laserscanner-topic.
What can i do to do the "callback-process and the calculation of the angles". I already implemented a method to do the average. The only thing I don't know is how to do the subscribe and writing to the vector loop only 20 times and then stop the method for calculating the average of the 20 values.
Thank you very much in advance.