publisher and subscriber in the same node
Hello. I want to write publisher and subscriber in the same node. So I wrote the program as follows. However, initially the subscriber 's callback function was ignored and only the publisher was executed. And after repeating with the while statement, only subscriber 's callback function was executed. Why is this program not working? Thank you.
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <cmath>
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
int n=0, m=0, exist[480][640];
double z[480][640];
void messageCallback(const sensor_msgs::Image::ConstPtr& msg)
{
int l=0, k[4];
double z[480][640];
for(n=0; n<4; ++n) {
k[n] = 0;
}
for(n=0; n<480; ++n) {
for(m=0; m<640; ++m) {
union {
unsigned char ch[4];
float count;
};
for(l=0; l<4; ++l) {
ch[l] = 0;
}
k[0] = 4*m+640*n;
k[1] = 4*m+1+640*n;
k[2] = 4*m+2+640*n;
k[3] = 4*m+3+640*n;
ch[0] = msg->data[k[0]];
ch[1] = msg->data[k[1]];
ch[2] = msg->data[k[2]];
ch[3] = msg->data[k[3]];
exist[n][m] = msg->data[k[3]];
if (exist[n][m] < 127) {
z[n][m] = count;
}
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "depth_subscriber_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/pico_flexx_link/depth_registered/image_raw", 1000, messageCallback);
PointCloud::Ptr msg (new PointCloud);
msg->header.frame_id = "/my_frame";
pcl_conversions::toPCL(ros::Time::now(), msg->header.stamp);
msg->height = 480;
msg->width = 640;
for(n=0; n<480; ++n) {
for(m=0; m<640; ++m) {
if(exist[n][m] < 127) {
float xp = z[n][m];
float yp = xp*(m/320.0-1.0)*tan(36.5*3.1416/180.0);
float zp = std::abs(xp)*(-n/240.0+1.0)*tan(29.5*3.1416/180.0);
msg->points.push_back (pcl::PointXYZ(xp, yp, zp));
}
}
}
pcl_conversions::toPCL(ros::Time::now(), msg->header.stamp);
pub.publish (msg);
ros::Rate loop_rate(0.1);
while (ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
Are you sending any data to the topic that you've subscribed to? The callback won't execute unless it receives data. Also, is this all of your code? I don't see where your publisher
pub
is defined.