Unable to access publisher in nodelet
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);
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 callLoop()
inonInit()
, you'll be blockingonInit()
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.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
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.