Refactor Turtlebot Follower.

turtlebot_follower/CMakeLists.txt

@@ -5,7 +5,10 @@ project(turtlebot_follower)
 find_package(catkin REQUIRED COMPONENTS nodelet roscpp visualization_msgs turtlebot_msgs depth_image_proc dynamic_reconfigure)
 find_package(Boost REQUIRED)
 
-generate_dynamic_reconfigure_options(cfg/Follower.cfg)
+generate_dynamic_reconfigure_options(
+  cfg/Follower.cfg
+  cfg/Centroid.cfg
+)
 
 catkin_package(
   INCLUDE_DIRS
@@ -25,7 +28,7 @@ include_directories(
 )
 
 ## Declare a cpp library
-add_library(${PROJECT_NAME} src/follower.cpp)
+add_library(${PROJECT_NAME} src/follower.cpp src/centroid.cpp)
 
 add_dependencies(${PROJECT_NAME}
   ${catkin_EXPORTED_TARGETS}

turtlebot_follower/cfg/Centroid.cfg

@@ -0,0 +1,49 @@
+#! /usr/bin/env python                                                                                          
+# Software License Agreement (BSD License)                                                                      
+#                                                                                                               
+# Copyright (c) 2009, Willow Garage, Inc.                                                                       
+# All rights reserved.                                                                                          
+#                                                                                                               
+# Redistribution and use in source and binary forms, with or without                                            
+# modification, are permitted provided that the following conditions                                            
+# are met:                                                                                                      
+#                                                                                                               
+#  * Redistributions of source code must retain the above copyright                                             
+#    notice, this list of conditions and the following disclaimer.                                              
+#  * Redistributions in binary form must reproduce the above                                                    
+#    copyright notice, this list of conditions and the following                                                
+#    disclaimer in the documentation and/or other materials provided                                            
+#    with the distribution.                                                                                     
+#  * Neither the name of Willow Garage, Inc. nor the names of its                                               
+#    contributors may be used to endorse or promote products derived                                            
+#    from this software without specific prior written permission.                                              
+#                                                                                                               
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS                                           
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT                                             
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS                                             
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE                                                
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,                                           
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,                                          
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                                              
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER                                              
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT                                            
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN                                             
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE                                               
+# POSSIBILITY OF SUCH DAMAGE.                                                                                   
+
+
+PACKAGE='turtlebot_follower'
+
+import math
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("min_x", double_t, 0, "The minimum x position of the points in the box.", -0.20, -3.0,  3.0)
+gen.add("max_x", double_t, 0, "The maximum x position of the points in the box.",  0.20, -3.0, 3.0)
+gen.add("min_y", double_t, 0, "The minimum y position of the points in the box.", 0.10, -1.0, 3.0)
+gen.add("max_y", double_t, 0, "The maximum y position of the points in the box.", 0.50, -1.0, 3.0)
+gen.add("max_z", double_t, 0, "The maximum z position of the points in the box.", 0.8, 0.0, 3.0)
+
+
+exit(gen.generate(PACKAGE, "turtlebot_follower_centroid_dynamic_reconfigure", "Centroid"))

turtlebot_follower/cfg/Follower.cfg

@@ -39,11 +39,6 @@ import math
 from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
-gen.add("min_x", double_t, 0, "The minimum x position of the points in the box.", -0.20, -3.0,  3.0)
-gen.add("max_x", double_t, 0, "The maximum x position of the points in the box.",  0.20, -3.0, 3.0)
-gen.add("min_y", double_t, 0, "The minimum y position of the points in the box.", 0.10, -1.0, 3.0)
-gen.add("max_y", double_t, 0, "The maximum y position of the points in the box.", 0.50, -1.0, 3.0)
-gen.add("max_z", double_t, 0, "The maximum z position of the points in the box.", 0.8, 0.0, 3.0)
 gen.add("goal_z", double_t, 0, "The distance away from the robot to hold the centroid.", 0.6, 0.0, 3.0)
 gen.add("x_scale", double_t, 0, "The scaling factor for translational robot speed.", 1.0, 0.0, 3.0)
 gen.add("z_scale", double_t, 0, "The scaling factor for rotational robot speed.", 5.0, 0.0, 10.0)

turtlebot_follower/launch/follower.launch

@@ -2,29 +2,26 @@
   The turtlebot people (or whatever) follower nodelet.
  -->
 <launch>
-  <arg name="simulation" default="false"/>
+  <arg name="simulation"    default="false"/>
+  <arg name="generate_goal" default="true"/> <!-- Use the default goal point generation -->
+
+  <!-- Generate Goal: Load camera and centroid goal generation nodelets -->
+  <!-- Else: Assume goal generation is loaded elsewhere -->
+  <include if="$(arg generate_goal)" file="$(find turtlebot_follower)/launch/includes/centroid.launch">
+	<arg name="simulation"  value="$(arg simulation)"/>
+  </include>
+
+  <node pkg="nodelet" type="nodelet" name="follower_nodelet_manager" args="manager"/>
+
   <group unless="$(arg simulation)"> <!-- Real robot -->
     <include file="$(find turtlebot_follower)/launch/includes/velocity_smoother.launch.xml">
       <arg name="nodelet_manager"  value="/mobile_base_nodelet_manager"/>
       <arg name="navigation_topic" value="/cmd_vel_mux/input/navi"/>
     </include>
-
-    <include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
-      <arg name="rgb_processing"                  value="true"/>  <!-- only required if we use android client -->
-      <arg name="depth_processing"                value="true"/>
-      <arg name="depth_registered_processing"     value="false"/>
-      <arg name="depth_registration"              value="false"/>
-      <arg name="disparity_processing"            value="false"/>
-      <arg name="disparity_registered_processing" value="false"/>
-      <arg name="scan_processing"                 value="false"/>
-    </include>
   </group>
   <group if="$(arg simulation)">
-    <!-- Load nodelet manager for compatibility -->
-    <node pkg="nodelet" type="nodelet" ns="camera" name="camera_nodelet_manager" args="manager"/>
-
     <include file="$(find turtlebot_follower)/launch/includes/velocity_smoother.launch.xml">
-      <arg name="nodelet_manager"  value="camera/camera_nodelet_manager"/>
+      <arg name="nodelet_manager"  value="follower_nodelet_manager"/>
       <arg name="navigation_topic" value="cmd_vel_mux/input/navi"/>
     </include>
   </group>
@@ -37,20 +34,12 @@
 
   <include file="$(find turtlebot_follower)/launch/includes/safety_controller.launch.xml"/>
 
-  <!--  Real robot: Load turtlebot follower into the 3d sensors nodelet manager to avoid pointcloud serializing -->
-  <!--  Simulation: Load turtlebot follower into nodelet manager for compatibility -->
   <node pkg="nodelet" type="nodelet" name="turtlebot_follower"
-        args="load turtlebot_follower/TurtlebotFollower camera/camera_nodelet_manager">
+        args="load turtlebot_follower/TurtlebotFollower follower_nodelet_manager">
     <remap from="turtlebot_follower/cmd_vel" to="follower_velocity_smoother/raw_cmd_vel"/>
-    <remap from="depth/points" to="camera/depth/points"/>
     <param name="enabled" value="true" />
     <param name="x_scale" value="7.0" />
     <param name="z_scale" value="2.0" />
-    <param name="min_x" value="-0.35" />
-    <param name="max_x" value="0.35" />
-    <param name="min_y" value="0.1" />
-    <param name="max_y" value="0.5" />
-    <param name="max_z" value="1.2" />
     <param name="goal_z" value="0.6" />
   </node>
   <!-- Launch the script which will toggle turtlebot following on and off based on a joystick button. default: on -->

turtlebot_follower/launch/includes/centroid.launch

@@ -0,0 +1,34 @@
+<!--
+  The turtlebot people (or whatever) follower nodelet.
+ -->
+<launch>
+  <arg name="simulation"    default="false"/>
+  <group unless="$(arg simulation)"> <!-- Real robot -->
+      <include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
+      <arg name="rgb_processing"                  value="true"/>  <!-- only required if we use android client -->
+      <arg name="depth_processing"                value="true"/>
+      <arg name="depth_registered_processing"     value="false"/>
+      <arg name="depth_registration"              value="false"/>
+      <arg name="disparity_processing"            value="false"/>
+      <arg name="disparity_registered_processing" value="false"/>
+      <arg name="scan_processing"                 value="false"/>
+    </include>
+  </group>
+  <group if="$(arg simulation)">
+    <!-- Load nodelet manager for compatibility -->
+    <node pkg="nodelet" type="nodelet" ns="camera" name="camera_nodelet_manager" args="manager"/>
+  </group>
+
+  <!--  Real robot: Load centroid goal generator into the 3d sensor's nodelet manager to avoid pointcloud serializing -->
+  <!--  Simulation: Load centroid goal generator into nodelet manager for compatibility -->
+  <node pkg="nodelet" type="nodelet" name="turtlebot_follower_centroid"
+        args="load turtlebot_follower/Centroid camera/camera_nodelet_manager">
+    <remap from="turtlebot_follower_centroid/goal" to="turtlebot_follower/goal"/>
+    <remap from="depth/points" to="camera/depth/points"/>
+    <param name="min_x" value="-0.35" />
+    <param name="max_x" value="0.35" />
+    <param name="min_y" value="0.1" />
+    <param name="max_y" value="0.5" />
+    <param name="max_z" value="1.2" />
+  </node>
+</launch>

turtlebot_follower/plugins/nodelets.xml

@@ -1,7 +1,12 @@
 <library path="lib/libturtlebot_follower">
   <class name="turtlebot_follower/TurtlebotFollower" type="turtlebot_follower::TurtlebotFollower" base_class_type="nodelet::Nodelet">
     <description>
-      The turtlebot people follower node.
+      The turtlebot people and object follower nodelet.
+    </description>
+  </class>
+  <class name="turtlebot_follower/Centroid" type="turtlebot_follower::Centroid" base_class_type="nodelet::Nodelet">
+    <description>
+      The centroid calculation nodelet used by Turtlebot Follower.
     </description>
   </class>
 </library>