Ask Your Question
0

publisher and subscriber in the same node

asked 2017-07-27 11:42:05 -0500

Akitoshi gravatar image

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;
}
edit retag flag offensive close merge delete

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.

jayess gravatar image jayess  ( 2017-07-27 20:00:54 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-07-28 00:04:56 -0500

Akitoshi gravatar image

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);
edit flag offensive delete link more

Comments

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

jayess gravatar image jayess  ( 2017-07-28 00:17:20 -0500 )edit

@jayess Ok, I will.

Akitoshi gravatar image Akitoshi  ( 2017-07-28 01:05:32 -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: 2017-07-27 11:42:05 -0500

Seen: 764 times

Last updated: Jul 28 '17