Nodelet Callback Not Working
My nodelet code is
PLUGINLIB_EXPORT_CLASS(example_pkg::MyNodeletClass, nodelet::Nodelet)
namespace example_pkg
{
void MyNodeletClass::messageCb(const std_msgs::UInt8::ConstPtr& msg)
{
ROS_INFO("NODE 1 CALLBACK");
}
void MyNodeletClass::onInit()
{
ros::NodeHandle n = getMTPrivateNodeHandle();
ROS_INFO("NODELET 1 INITIALIZATION");
n.subscribe("msg_in", 10, &MyNodeletClass::messageCb, this);
}
}
and my main node code is
int main(int argc, char **argv)
{
ros::init(argc, argv, "main");
nodelet::M_string remap(ros::names::getRemappings());
nodelet::V_string nargv;
nodelet::Loader nodelet1;
nodelet1.load("one", "example_pkg/MyNodeletClass", remap, nargv);
ROS_INFO("I AM HERE");
ros::spin();
return 0;
}
After I run the main code, I do rostopic pub msgin stdmsgs/UInt8 1 but the output remains
[ INFO] [1542015803.740819742]: Initializing nodelet with 6 worker threads.
[ INFO] [1542015803.789365470]: INITIALIZATION OF NODE 1 COMPLETE
[ INFO] [1542015803.821358176]: I AM HERE
I have also tried to use a launch file as below
<launch>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"
></node>
<node pkg="nodelet" type="nodelet" name="MyNodeletClass" args="load example_pkg/MyNodeletClass standalone_nodelet" output="screen"></node>
</launch>
and after launching that, I run the same rostopic pub msgin stdmsgs/UInt8 1 command and get the following output
[ INFO] [1542019185.420793556]: Initializing nodelet with 6 worker threads.
[ INFO] [1542019185.440188632]: NODELET 1 INITIALIZATION
Why isn't the nodelet going into callback? I am not supposed to put ros::spin() in the onInit() function of the nodelet right? Thanks in advance.
Asked by Galadriel1233 on 2018-11-12 05:01:30 UTC
Answers
I'm pretty sure the issue is with the code in onInit()
, you call n.subscribe(..)
, but you don't actually store the ros::Subscriber
that is returned by that anywhere.
As soon as the object returned goes out of scope (at the end of onInit()
), the subscription is destroyed, and the callback not called any longer.
Asked by gvdhoorn on 2018-11-12 11:28:08 UTC
Comments
Thanks that was absolutely the mistake. Solved!
Asked by Galadriel1233 on 2018-11-12 23:28:56 UTC
Comments
Just checking something first: you use
getMTPrivateNodeHandle()
and advertise a topic on that.What is the output of
rostopic list
when everything is running?Additionally: why are you not using a regular nodelet manager?
Asked by gvdhoorn on 2018-11-12 05:12:20 UTC
When I run the rostopic pub command this is the output.
/msg_in /rosout /rosout_agg
I am also trying to use a launch file with a regular nodelet manager. I edited the question. When I do that and run rostopic list I get
/msg_in /rosout /rosout_agg /standalone_nodelet/bond
Thanks @gvdhoorn
Asked by Galadriel1233 on 2018-11-12 05:42:46 UTC