Commits

Patrick Bouffard committed 31a99bd

- mux nodelet working; use this instead of ordinary mux
- nodelet manager gets its own launch (xml) file

Comments (0)

Files changed (4)

starmac_common/starmac_tools/CMakeLists.txt

 #  Release        : w/o debug symbols, w/ optimization
 #  RelWithDebInfo : w/ debug symbols, w/ optimization
 #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
+#set(ROS_BUILD_TYPE Debug)
 
 rosbuild_init()
 

starmac_common/starmac_tools/nodes/mux.cpp

 class MuxNodelet : public nodelet::Nodelet
 {
 public:
-  ros::NodeHandle *g_node;
+  ros::NodeHandle g_node;
+  ros::NodeHandle g_pnh;
   bool g_advertised;
   string g_output_topic;
   ros::Publisher g_pub;
   ros::ServiceServer ss_list;
   ros::ServiceServer ss_del;
 
-  MuxNodelet () : g_node(0), g_advertised(false), g_selected(g_subs.end())
+  MuxNodelet () :  g_advertised(false), g_selected(g_subs.end())
   {}
 
   //int main(int argc, char **argv)
     nh_priv.param("topic_name", topic_name, topic_name);
 
     ros::NodeHandle n;
-    //g_node = &getMTNodeHandle();
+    g_node = getMTNodeHandle();
+    g_pnh = getMTPrivateNodeHandle();
     g_output_topic = topic_name;
     // Put our API into the "mux" namespace, which the user should usually remap
     //ros::NodeHandle mux_nh("mux");
     if (!g_advertised)
     {
       ROS_INFO("advertising");
-      g_pub = msg->advertise(*g_node, g_output_topic, 10);
+      g_pub = msg->advertise(g_pnh, g_output_topic, 10);
       g_advertised = true;
     }
     if (s == g_selected->msg)
     sub_info.msg = new ShapeShifter;
     try
     {
-      sub_info.sub = g_node->subscribe<ShapeShifter> (req.topic, 10, boost::bind(&MuxNodelet::in_cb, this, _1, sub_info.msg));
+      sub_info.sub = g_node.subscribe<ShapeShifter> (req.topic, 10, boost::bind(&MuxNodelet::in_cb, this, _1, sub_info.msg));
     }
     catch (ros::InvalidNameException& e)
     {

starmac_flyer/flyer_common/launch/systems/control.xml

   <arg name="machine" default="local"/>
   <arg name="max_alt_cmd" default="1.5"/>
   <arg name="teleop_use_udp" default="true"/>
+  <arg name="nodelet_manager" default="manager"/>
   
   <!-- for debugging: 
       launch-prefix="xterm -e gdb -x ~/gdbrun -args" 
   <!-- CONTROLLER -->
 
   <group>
-    <node pkg="nodelet" type="nodelet" name="manager" args="manager" machine="$(arg machine)"
-      >
-      <rosparam>
-        # Rule of thumb: at least one worker thread per nodelet loaded into the manager
-        num_worker_threads: 40
-        </rosparam>
-    </node>
     
-    <node pkg="nodelet" type="nodelet" name="controller" args="load flyer_controller/Controller manager" machine="$(arg machine)"/>
+    <node pkg="nodelet" type="nodelet" name="controller" 
+          args="load flyer_controller/Controller $(arg nodelet_manager)" 
+          machine="$(arg machine)"/>
     <remap from="teleop_flyer/estop" to="asctec/ESTOP"/>
     <remap from="teleop_flyer/controller_cmd" to="controller/cmd"/>
     
-    <node name="teleop_flyer" pkg="nodelet" type="nodelet" args="load flyer_controller/TeleopFlyer manager" machine="$(arg machine)">
+    <node name="teleop_flyer" pkg="nodelet" type="nodelet" 
+          args="load flyer_controller/TeleopFlyer $(arg nodelet_manager)" 
+          machine="$(arg machine)">
       <param name="max_interval" value="0.75"/>
       <param name="use_udp" value="$(arg teleop_use_udp)"/>
     </node>
     <group>
 
       <!-- CONTROL MODES -->
-      <node name="control_mode_idle" pkg="nodelet" type="nodelet" args="load flyer_controller/ControlModeIdle manager" machine="$(arg machine)">
+      <node name="control_mode_idle" pkg="nodelet" type="nodelet" 
+            args="load flyer_controller/ControlModeIdle $(arg nodelet_manager)" 
+            machine="$(arg machine)">
       </node>
-      <node name="control_mode_attitude" pkg="nodelet" type="nodelet" args="load flyer_controller/ControlModeAttitude manager" machine="$(arg machine)">
+      <node name="control_mode_attitude" pkg="nodelet" type="nodelet" 
+            args="load flyer_controller/ControlModeAttitude $(arg nodelet_manager)" 
+            machine="$(arg machine)">
         <rosparam>
           external_command_frame: true
           external_frame_heading: 0.0
           </rosparam>
           <param name="max_alt_cmd" value="$(arg max_alt_cmd)"/>
       </node>
-      <node name="control_mode_hover" pkg="nodelet" type="nodelet" args="load flyer_controller/ControlModeHover manager" machine="$(arg machine)">
+      <node name="control_mode_hover" pkg="nodelet" type="nodelet"
+            args="load flyer_controller/ControlModeHover $(arg nodelet_manager)" 
+            machine="$(arg machine)">
         <rosparam>
           KP: 30.0
           KI: 10.0
           </rosparam>
           <param name="max_alt_cmd" value="$(arg max_alt_cmd)"/>
       </node>
-      <node name="control_mode_autosequence" pkg="nodelet" type="nodelet" args="load flyer_controller/ControlModeAutosequence manager" machine="$(arg machine)">
+      <node name="control_mode_autosequence" pkg="nodelet" type="nodelet" 
+            args="load flyer_controller/ControlModeAutosequence $(arg nodelet_manager)" 
+            machine="$(arg machine)">
         <rosparam>
           KP: 30.0
           KI: 5.0
     </group>
       
     <!-- CONTROL MODE MUX -->
-    <node pkg="topic_tools" type="mux" name="controller_mux" args="mux:=controller_mux controller_mux/output dummy" machine="$(arg machine)"/>
+    <node if="false" pkg="topic_tools" type="mux" name="controller_mux" 
+          args="mux:=controller_mux controller_mux/output dummy" 
+          machine="$(arg machine)"/>
+    
+    <node name="controller_mux" pkg="nodelet" type="nodelet" 
+            args="load starmac_tools/MuxNodelet $(arg nodelet_manager)" 
+            machine="$(arg machine)">
+          <param name="max_alt_cmd" value="$(arg max_alt_cmd)"/>
+      </node>
+          
   </group>
 </launch>

starmac_flyer/flyer_common/launch/systems/nodelet_manager.xml

+<launch>
+  <arg name="name" default="manager" />
+  <arg name="machine" default="local"/>
+  <arg name="num_worker_threads" default="40"/>
+
+  <!-- for debugging: 
+      launch-prefix="xterm -e gdb -x ~/gdbrun -args" 
+       or:  
+      launch-prefix="screen -d -m gdb -x ~/gdbrun -args" -->
+
+  <node pkg="nodelet" type="nodelet" name="$(arg name)" 
+        args="manager" machine="$(arg machine)">
+        <param name="num_worker_threads" value="$(arg num_worker_threads)"/>
+  </node>
+  
+
+</launch>