Robotics StackExchange | Archived questions

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

Asked by Akitoshi on 2017-07-27 11:42:05 UTC

Comments

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.

Asked by jayess on 2017-07-27 20:00:54 UTC

Answers

Thank @jayess. As you said pulisher was missing. The real program is as follows. In my program I subscribe depth values from Gazebo and are planning to publish pointclouds based on that data. However, although the program was activated while Gazebo was running, the callback function was ignored at first. And from the second lap only the callback function was called and the publisher stopped working.

ros::Subscriber sub = nh.subscribe("/pico_flexx_link/depth_registered/image_raw", 1000, messageCallback);

ros::Publisher pub = nh.advertise<PointCloud> ("points2", 1); /*This code was missing*/

PointCloud::Ptr msg (new PointCloud);

Asked by Akitoshi on 2017-07-28 00:04:56 UTC

Comments

Please update your question with this information instead of posting an update to it as an answer.

Asked by jayess on 2017-07-28 00:17:20 UTC

@jayess Ok, I will.

Asked by Akitoshi on 2017-07-28 01:05:32 UTC