Ask Your Question
0

Callback of Subscriber as a Function of a Class isn't Called

asked 2020-02-10 14:01:56 -0600

kiro26 gravatar image

updated 2020-02-11 00:27:17 -0600

Hi all, I was trying to make a class for my program handler including subscriber and publisher. But I'm stuck since 2 weeks without no progress. Somehow the callback function isn't called...

my main file

#include <ros/ros.h>
#include "pointcloud_reader_handler.h"

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

ros::init(argc, argv, "pointcloud_reader");
ros::NodeHandle private_node_handle_("~");
// class object which handle the package
pointcloud_reader_handler myReaderHandle(private_node_handle_);

// ros spin with the rate time
ros::Rate naptime(myReaderHandle.getNodeRate());

// Main Loop
while(ros::ok())
{
    myReaderHandle.run();
    // sleep for given rate(time)
    ros::spinOnce();
    naptime.sleep();
}
return 0;
 }

my pointcloud reader handler

#include <ros/ros.h>
#include "pointcloud_reader_handler.h"

// Constructor
pointcloud_reader_handler::pointcloud_reader_handler(ros::NodeHandle &nodeHandle):
nodeHandle_(nodeHandle) {
    ROS_INFO("Construction Handle");
    loadParameters();
    ROS_INFO("Subscribing topic starting");
    subscribeToTopics();
    publishToTopics();
}

// Getters
int pointcloud_reader_handler::getNodeRate() const {return node_rate_;}

// Methods
void pointcloud_reader_handler::loadParameters() {
// add parameters from cfg file here
}

void pointcloud_reader_handler::subscribeToTopics() {
    ROS_INFO("Subscribing topic");
    pointcloud_reader_handler::lidarSubscriber_ = nodeHandle_.subscribe<sensor_msgs::PointCloud2>
    (lidar_input_topic_name_, input_message_queue_, 
    &pointcloud_reader_handler::lidarReadingCallback, this);
    }

void pointcloud_reader_handler::publishToTopics() {
    ROS_INFO("publish to topics");
    lidarPublisher_ = nodeHandle_.advertise<sensor_msgs::PointCloud2>(lidar_cone_filter_topic_name_, output_filter_message_queue_);
    detectionPublisher_ = nodeHandle_.advertise<fsd_common_msgs::Detection>(lidar_cone_detection_topic_name_, output_detection_message_queue_);
    detectionLidarPublisher_ = nodeHandle_.advertise<sensor_msgs::PointCloud2>(lidar_cone_detection_pointcloud_topic_name_, output_detection_pointcloud_message_queue_);
}

void pointcloud_reader_handler::run() {
     // do stuff here
}

void pointcloud_reader_handler::lidarReadingCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg) {
    ROS_INFO("Filtering Lidar Point Cloud");
    pointCloudPreProcessing_.updateInputCloud(pointCloudMsg);
}

Some small detail:

  • There is no error in compiling.
  • The topic that I want to subscribe is there
  • The lidarSubscriber_ is declared (private/public, both doesnt work) in the class.

The output I get with rosrun is:

[ INFO] [1581362020.241452441]: Construction Handle
[ INFO] [1581362020.244867156]: Subscribing topic starting
[ INFO] [1581362020.244876716]: Subscribing topic
[ INFO] [1581362020.245867283]: publish to topics
perception_lidar: /usr/include/boost/smart_ptr/shared_ptr.hpp:728: typename boost::detail::sp_dereference<T>::type boost::shared_ptr<T>::operator*() const [with T = const sensor_msgs::PointCloud2_<std::allocator<void> >; typename boost::detail::sp_dereference<T>::type = const sensor_msgs::PointCloud2_<std::allocator<void> >&]: Assertion `px != 0' failed.

So, as I debuged the code and also with print message, I noticed that the function is called which prints

[ INFO] [1581362020.244876716]:0 Subscribing topic

but not the message

Filtering Lidar Point Cloud

it jumps directly to publish topic. Means the callback function is not called.

Please if anyone can help. Thank you guys.

EDIT: header file

#ifndef SRC_POINTCLOUD_READER_HANDLER_H
#define SRC_POINTCLOUD_READER_HANDLER_H

#include "PointCloudPreProcessing.h"
#include "ObjectDetection.h"
#include <sensor_msgs/PointCloud2.h>

class pointcloud_reader_handler {
public:

    // Constructor
    pointcloud_reader_handler(ros::NodeHandle &nodeHandle);

    // Getters
    int getNodeRate() const;

    // Methods
    void loadParameters();
    void subscribeToTopics();
    void publishToTopics();
    void run();
    ros::Publisher lidarPublisher_;
    ros::Subscriber lidarSubscriber_;

private:
    ros::NodeHandle nodeHandle_;
    ros::Publisher detectionPublisher_;
    ros::Publisher detectionLidarPublisher_;

    void lidarReadingCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg);

    std::string lidar_input_topic_name_;
    std::string lidar_cone_filter_topic_name_;
    std::string lidar_cone_detection_topic_name_;
    std::string lidar_cone_detection_pointcloud_topic_name_;
    std::string frame_id_;

   // some other variables

    PointCloudPreProcessing pointCloudPreProcessing_;
    sensor_msgs::PointCloud2Ptr points_filtered_;
};

#endif //SRC_POINTCLOUD_READER_HANDLER_H
edit retag flag offensive close merge delete

Comments

Can you please publish content of pointcloud_reader_handler.h ? This will help debugging. I hit this sitation a few times in the past so I can take a quick look and try to run your example

viktor.holova gravatar imageviktor.holova ( 2020-02-10 17:50:19 -0600 )edit

I include the header in the edited question.

kiro26 gravatar imagekiro26 ( 2020-02-11 00:27:53 -0600 )edit

So I have solved the error in pointer. I used a variable in other class to store lidar points and with sensor_msgs::PointCloud2ConstPtr I couldn't just use ( new Pointcloud2).

But it should be:

sensor_msgs::PointCloud2ConstPtr point_cloud_input_ = boost::make_shared<const sensor_msgs::PointCloud2>();

But it still doesn't subscribe to the given topic.

kiro26 gravatar imagekiro26 ( 2020-02-11 04:22:45 -0600 )edit

it jumps directly to publish topic. Means the callback function is not called.

It is actually expected to have the message publish to topics right after Subscribing topic, when you define the subscriber you don't call the callback function so you don't have to expect Filtering Lidar Point Cloud.

Your question might be unclear about your issue : Do you have an issue because you expect to see Filtering Lidar Point Cloud right after the definition of the subscriber or your callback is never called while data is published on lidar_input_topic_name_ (meaning you have an output when using rostopic echo) ?

Delb gravatar imageDelb ( 2020-02-11 05:01:22 -0600 )edit

I have an input of the lidar running from other package and expect for the callback to be executed while the input is received.

kiro26 gravatar imagekiro26 ( 2020-02-11 05:21:22 -0600 )edit

I have an input of the lidar running from other package

Just to be sure : Do you actually have data received with rostopic echo ? Also, where do you set all the topic names ? And to which value ? Particularly lidar_input_topic_name_, are you sure it's the same as the actual lidar topic ?

Delb gravatar imageDelb ( 2020-02-11 07:40:08 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-02-11 16:25:20 -0600

viktor.holova gravatar image

The reason why your callback is not called is topic names mismatch. Let me explain.

Let's say your variable lidar_input_topic_name_ contains value echo. You probably expect to subscribe to the topic name /echo. However, in reality ROS subscribes to <your node name>/echo because you pass private_node_handle_("~") into nodeHandle_ variable of the class constructor.

This causes all subscriptions to have your node name in front of topic names. See http://wiki.ros.org/roscpp/Overview/N... for explanation

So minimal way to fix it is to change

ros::NodeHandle private_node_handle_("~");

to

ros::NodeHandle private_node_handle_;

I just tested on my machine and it works.

P.S. By the way, my usual practice is to pass two NodeHandle variables into my constructors - one regular (i.e. without ~) and one private

Then I use regular handle for all of my publishers, subscribers and global parameters and I use private handle to get node's private parameters with <private nh>.param() function

edit flag offensive delete link more

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: 2020-02-10 14:01:56 -0600

Seen: 73 times

Last updated: Feb 11