Ask Your Question
0

Get 10 values of topic I'm subscribing to

asked 2018-02-17 11:05:12 -0500

DBServ gravatar image

updated 2018-02-19 12:37:34 -0500

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-02-18 04:53:36 -0500

gvdhoorn gravatar image

updated 2018-02-18 04:54:36 -0500

Edit: see #q261816 for a duplicate of your question, but with an answer in Python.


Simplest (but with a bit of duplicated effort): create a std::list or std::vector that holds incoming samples (ie: PointClouds). Then every so often (ie: after N new samples), run your averaging algorithm.

To make it nice: add a sliding window approach (ie: throw away/pop the M last samples after each averaging run).


With a bit more reuse: make use of the message_filters package. For a simple cache, you could look at the Cache filter.

edit flag offensive delete link more

Comments

Thank you very much for the idea with cache and vector. I still got a history-vector where I add the value of the pointcloud but right now only in a open loop. I want to stop the loop for subscribing and calculation and build after that the average of the values. I add the full code above!

DBServ gravatar imageDBServ ( 2018-02-19 01:24:17 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-02-17 11:05:12 -0500

Seen: 86 times

Last updated: Feb 19 '18