Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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;
}