Ask Your Question
0

Publish message after filtering subscribed topic

asked 2018-03-09 12:02:58 -0500

Georgee gravatar image

Hi there, I want to subscribe to a PointCloud topic, filter it and publish it later on a different topic in real time. What I have so far is this:

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);   //TODO: Avoid global variables. Is there another way?

 void subCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& input){
  pcl::copyPointCloud(*input,*cloud);
  std::cerr<<"Cloud size is "<< cloud->points.size()<<std::endl;
}

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

      ros::init(argc, argv, "plane_extraction");


      //Necessary pointclouds
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>),
                                          cloud_projected (new pcl::PointCloud<pcl::PointXYZ>),
                                          cloud_p (new pcl::PointCloud<pcl::PointXYZ>),
                                          cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
      std::vector<pcl::PointCloud<pcl::PointXYZ> > results;


      //Only one Node Handle for both subscribing and publishing
      ros::NodeHandle nh;

      //Publishers array (one for each topic). Each topic represents a segmented plane

      std::vector<ros::Publisher> publishers;

      while(ros::ok()){

            sub=nh.subscribe("/camera/depth/points", 5, subCallback);

           /*Filters and stuff
           ******************
           ******************
           ******************
           ******************/

            for(int j=0;j<i;j++){
              publishers[j].publish(results[j]);
              std:cerr<<"There are "<<j<<" publishers"<<std::endl;
            }
            ros::spinOnce();
      }

      return(0);

}

I don't know if the while(ros::ok()) spinOnce() is incorrect or what, but I don't find the way to do it. The message inside the callback appears randomly, and when it does, it gets stuck there, as if it never left the callback.

Thanks in advance.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2018-03-09 15:08:59 -0500

Gayan Brahmanage gravatar image

Hi, As Cerin mentioned, you don't need to put nh.subscribe("/camera/depth/points", 5, subCallback); in thewhileloop. Because, it uses to make subscriber object, so you just need to do it in initialization of your main function.

Your call back function is called whenever your subscribed topic is updated. Otherwise, it will not be called.

You can publish your new topic in a while loop but it can be updated only when your callback function is called. You can use a global variable to save and modify your data and publish it either in the while loop or callback function. I have attached a simple example here. Hope this will help you.

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/LaserScan.h"
#include <gayan2Dmapping/gayan2Dmappingmsgs.h> 
#include <gayan2Dmapping/fakemsg.h>
#include <gayan2Dmapping/processedscan.h>
#include <sstream>


ros::Publisher pub_processedscan;
ros::Subscriber scanSub;

double x=0,y=0,z=0,theta=0;
double scanx[640],scany[640];

void processLaserScan(const sensor_msgs::LaserScan::ConstPtr& scan){

    x = scan->ranges[320]; 

    float sizenum_ranges = scan->ranges.size(); 
    ROS_INFO("Range[%f], #[%f]",x,sizenum_ranges);
//-------------------------------------------------------------------   
    gayan2Dmapping::processedscan msg;
    int i=0;
    for(i=0;i<640;++i){
        if(i<320){
            int n=320-i;
            scanx[i]=(scan->ranges[i])*cos(n*scan->angle_increment);
            scany[i]=(scan->ranges[i])*sin(n*scan->angle_increment);

        }
        else {
            int n=-320+i;
            scanx[i]=(scan->ranges[i])*cos(n*scan->angle_increment);
            scany[i]=(scan->ranges[i])*sin(-n*scan->angle_increment);

        }
    }

}



void publishprocessedscan(){
    gayan2Dmapping::processedscan msg;
    int i=0;
    for(i=0;i<640;++i){
        msg.processedscan[i]=scanx[i];
        msg.processedscany[i]=scany[i];
    }
    pub_processedscan.publish(msg);
}


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

    ros::init(argc, argv, "gayan2Dmapping");
    ros::NodeHandle n;
    ros::Rate loop_rate(100);


    scanSub=n.subscribe<sensor_msgs::LaserScan>("/scan",1000,processLaserScan);
    pub_processedscan = n.advertise<gayan2Dmapping::processedscan>("processedscan", 1000);

    while(ros::ok()){

                ros::spinOnce();
                loop_rate.sleep();  
                publishprocessedscan();
    }

  return 0;
}
edit flag offensive delete link more

Comments

Thank you. That was absolutely precise and effective. It works now!

Georgee gravatar imageGeorgee ( 2018-03-10 05:13:07 -0500 )edit
0

answered 2018-03-09 13:03:35 -0500

Cerin gravatar image

I don't think you need to call nh.subscribe("/camera/depth/points", 5, subCallback); from inside the while loop. That just re-subscribes to the topic over and over again. I'm not sure what kind of noise that creates in ROS's backend, but I'm guessing it's unnecessary at best.

Also, why are you publishing in the while? Why not publish directly from inside your callback if the point cloud message fits your criteria?

To stop your main thread from exiting, I think all you need is ros::spin();

edit flag offensive delete link more

Comments

First of all, thank you for your response. Just to be sure to understand what you say: I should take the nh.subscribe out of the loop, and then do a while(ros::ok()) ros::spin() instead of spinOnce(), right? Should I have another global variable for the filtered cloud so I can publish outside main?

Georgee gravatar imageGeorgee ( 2018-03-09 14:02:05 -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-03-09 12:02:58 -0500

Seen: 294 times

Last updated: Mar 09 '18