Using allow_concurrent_options as advised from ros/nodelet_core#77 (c…

…omment)
lucasw · Sep 23, 2021 · 6160a2f · 6160a2f
1 parent d3668e0
commit 6160a2f
Show file tree

Hide file tree

Showing 4 changed files with 27 additions and 11 deletions.
diff --git a/nodelet_demo/include/nodelet_demo/nodelet_demo.h b/nodelet_demo/include/nodelet_demo/nodelet_demo.h
@@ -56,7 +56,10 @@ class NodeletDemo : public nodelet::Nodelet
   boost::recursive_mutex dr_mutex_;
   #endif
 
-  float callback_delay_;
+  // set true if wanting to track cpu usage
+  bool busy_wait_ = false;
+
+  float callback_delay_ = 1.0;
   void callback(const std_msgs::Float32ConstPtr& msg);
   // ros::Timer timer_;
   // void update(const ros::TimerEvent& e);

diff --git a/nodelet_demo/launch/demo.launch b/nodelet_demo/launch/demo.launch
@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="period" default="0.1" />
+  <arg name="busy_wait" default="false" />
   <arg name="mt_callback" default="false" />
   <arg name="use_demux" default="false" />
   <arg name="use_input2" default="false" />
@@ -71,6 +72,7 @@ It's odd that demux uses private namespaces for the inputs and outputs
         output="screen">
       <param name="callback_delay" value="1.0" />
       <param name="mt_callback" value="$(arg mt_callback)" />
+      <param name="busy_wait" value="$(arg busy_wait)" />
     </node>
 
     <!-- This kills the manager

diff --git a/nodelet_demo/scripts/gen_input.py b/nodelet_demo/scripts/gen_input.py
@@ -11,7 +11,7 @@
 
 period = rospy.get_param("~period", 0.8)
 
-count = rospy.get_param("~start", 0)
+count = rospy.get_param("~start", 1000)
 
 while not rospy.is_shutdown():
     count += 1

diff --git a/nodelet_demo/src/nodelet_demo.cpp b/nodelet_demo/src/nodelet_demo.cpp
@@ -37,7 +37,6 @@ namespace nodelet_demo
 {
 
 NodeletDemo::NodeletDemo() :
-  callback_delay_(1.0),
   spinner_(4)
 {
 }
@@ -57,23 +56,24 @@ void NodeletDemo::drCallback(
 
 void NodeletDemo::callback(const std_msgs::Float32ConstPtr& msg)
 {
-  ROS_INFO_STREAM("start, cpu id " << get_cpu_id() << ", count " << msg->data);
-  // set true if wanting to track cpu usage
-  bool busy_wait = true;
-  if (!busy_wait)
+  ROS_INFO_STREAM("start | msg " << msg->data << ", cpu id " << get_cpu_id());
+  // TODO(lucasw) cpu id can change after either of these waiting methods?
+  if (!busy_wait_)
   {
+    ROS_WARN_STREAM_ONCE("not using busy wait, ros sleeping");
     ros::Duration(callback_delay_).sleep();
   }
   else
   {
+    ROS_WARN_STREAM_ONCE("using busy wait");
     ros::Time t0 = ros::Time::now();
     while ((ros::Time::now() - t0).toSec() < callback_delay_)
     {
     }
   }
 
   pub_.publish(msg);
-  ROS_INFO_STREAM("done, cpu id " << get_cpu_id() << ", count " << msg->data);
+  ROS_INFO_STREAM("done  | msg " << msg->data << ", cpu id " << get_cpu_id());
 }
 
 #if 0
@@ -110,23 +110,34 @@ void NodeletDemo::onInit()
   pnh.getParam("output_queue_size", output_queue_size);
   pub_ = nh.advertise<std_msgs::Float32>("output", output_queue_size);
 
+  pnh.getParam("callback_delay", callback_delay_);
 #if 0
   server_.reset(new ReconfigureServer(dr_mutex_, pnh));
   dynamic_reconfigure::Server<nodelet_demo::DelayConfig>::CallbackType cbt =
       boost::bind(&NodeletDemo::callback, this, _1, _2);
   server_->setCallback(cbt);
 #endif
 
-  pnh.getParam("callback_delay", callback_delay_);
+  pnh.getParam("busy_wait", busy_wait_);
 
   int input_queue_size = 10;
   pnh.getParam("input_queue_size", input_queue_size);
 
-  sub1_ = nh.subscribe("input1", input_queue_size,
-      &NodeletDemo::callback, this);
+  // TODO(lucasw) setup sub2 like this also?
+  ros::SubscribeOptions so1;
+  so1.init<std_msgs::Float32>(
+      "input1",
+      input_queue_size,
+      boost::bind(&NodeletDemo::callback, this, _1));
+
+  so1.allow_concurrent_callbacks = true;
 
+  sub1_ = nh.subscribe(so1);
+#if 0
   sub2_ = nh.subscribe("input2", input_queue_size,
       &NodeletDemo::callback, this);
+#endif
+
 #if 0
   // this crashes the manager
   if (mt_callback)