Unable to access publisher in nodelet

asked 2023-03-16 17:31:47 -0600

robo_ninja gravatar image

I am writing a ROS nodelet wrapper class for my existing ROS node. My nodelet is processing image data and publishing an output image with bounding box. I am getting the following error which I assume means that I am publishing before initializing the publisher?!? I have initialized the image publisher in header file. How else I am supposed to initialize it? Can't my nodelet access the publisher? Error -

[FATAL] [1679000699.594996126]: ASSERTION FAILED
    file = /opt/ros/noetic/include/ros/publisher.h
    line = 71
    cond = false
    message = 
[FATAL] [1679000699.595018289]: Call to publish() on an invalid Publisher
[FATAL] [1679000699.595023792]: 
[FATAL] [1679000700.700172311]: Failed to load nodelet '/TestNodelet` of type `test_detect/TestNodelet` to manager `cam_nodelet_manager'

Following is my header file test.h:

// << All includes >>

namespace test {
class TestNode {
 public:
  TestNode(const std::string& cam);
  static TestNode& GetInstance(const std::string& cam = "");
  inline const ros::Publisher& GetImagePub(void) const {return imagePub;}
  bool init();
  const cv::Mat& Filter(const cv::Mat& image);
  const cv::Mat& DT(const cv::Mat& image);    
 private:
  ros::NodeHandle nh;
  ros::Subscriber imageSub;
  ros::Publisher imagePub;
  void InputImageCb(const sensor_msgs::Image::ConstPtr& msg);
  const std::string cam;
};
}

My ROS node looks like the following test_node.cpp:

#include <test/test_node.h>

namespace test {
TestNode::TestNode(const std::string& cam)
    : cam(cam), nh(){};
bool TestNode::init() {
  imageSub = nh.subscribe("/camera/image_raw", 10, &TestNode::InputImageCb, this);
  imagePub = nh.advertise<sensor_msgs::Image>("/camera/final_image", 100);
  return true;
}

void TestNode::InputImageCb(const sensor_msgs::Image::ConstPtr& msg) {
  cv_bridge::CvImageConstPtr cvPtr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
  TestNode::GetInstance().SetCurrentImage(cvPtr->image);
  }

const cv::Mat& TestNode::Filter(const cv::Mat& image) { 
  // code
  return mask;
}

const cv::Mat& TestNode::DT(const cv::Mat& image) {
  // code
  return image;
}

}

Now I am trying to wrap the test class in a nodelet class in the following way (test_nodelet.cpp) -

#include "test/test_node.h"
#include "nodelet/nodelet.h"
#include "pluginlib/class_list_macros.h"

namespace test {
class TestNodelet : public nodelet::Nodelet {
 public:
  TestNodelet(){};
  ~TestNodelet() {}

 private:
  boost::shared_ptr<TestNode> node_;

  virtual void onInit() {
    ros::NodeHandle& pnh = this->getMTPrivateNodeHandle();   
    std::string cam;
    pnh.getParam("camera_name", cam);
    node_ = boost::make_shared<TestNode>(cam);
    if (node_->init()) {
      NODELET_INFO_STREAM("Initialized");
    }
    Loop(cam);
  }

  static void Loop(const std::string& cam) {
    TestNode& node_ = node_.GetInstance(cam);

    while (ros::ok()) {
      ros::spinOnce();
        const cv::Mat& mask = node_.Filter(currentImage); // function in node class to filter
        const cv::Mat& bbox = node_.DT(mask); // function to publish bounding box
        sensor_msgs::ImagePtr msg = cv::bridge::CvImage(std_msgs::Header(), "mono8", bbox).toImageMsg();
        const ros::Publisher& imagePub = node_.GetImagePub();
        imagePub.publish(msg);
    }
  }
};
}

PLUGINLIB_EXPORT_CLASS(test::TestNodelet, nodelet::Nodelet);
edit retag flag offensive close merge delete

Comments

quick comment: you cannot "wrap a node in a nodelet". That's not going to get you any of the benefits nodelets offer. What you should do is write a nodelet, and when you want to use it "as a node", just load it into its own nodelet_manager.

You also cannot run a while(ros::ok()) loop like you do. You cannot call Loop() in onInit(), you'll be blocking onInit() forever, which will block the nodelet manager forever as well, bringing everything to a halt.

I know nodelets aren't easy, especially when just starting out, and underdocumented, so I'd suggest looking for some existing implementations. The nodelet wiki page has links to packages using nodelet. Click on the Used by section heading in the Package Links box on the right hand side of the page.

gvdhoorn gravatar image gvdhoorn  ( 2023-03-17 01:49:59 -0600 )edit

Thank you for clarifying that. I was following [kobuki controller wiki page] (http://wiki.ros.org/kobuki/Tutorials/...) and got the idea of wrapping a node in a nodelet class and thought it might be an easier route to not having to re-write the entire node as a nodelet again. But I understand the drawback and I will start creating a separate nodelet

robo_ninja gravatar image robo_ninja  ( 2023-03-17 02:52:15 -0600 )edit

got the idea of wrapping a node in a nodelet class and thought it might be an easier route to not having to re-write the entire node as a nodelet again

well as I wrote: you don't need to write something again. Convert what you have into a nodelet, then load it into a standalone nodelet_manager. If you load it together with other nodelets into a manager, it will behave as a nodelet. If you load it by itself, it will behave like a node.

Note though: you absolutely have to publish and subscribe using shared ptrs. See the documentation on intra-process publishing. If you don't do that, you won't get any of the benefits of something being a nodelet, even if you "rewrite" your node.

gvdhoorn gravatar image gvdhoorn  ( 2023-03-17 05:07:46 -0600 )edit