Added possibility to dynamically change parameters of the test_optim_…

…node using dynamic reconfigure. Fixed a wrong default-min-max tuple in the dynamic reconfigure config. Useful config and launch files are now added to cmake install.
zhenya-lei · May 21, 2015 · 05773d4 · 05773d4
1 parent 62f5c86
commit 05773d4
Show file tree

Hide file tree

Showing 7 changed files with 35 additions and 15 deletions.
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1,3 @@
+.svn
+.kdev4
+*.kdev4
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -225,6 +225,12 @@ install(FILES
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
+install(DIRECTORY
+  launch cfg
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+
 #############
 ## Testing ##
 #############

diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg
@@ -167,10 +167,6 @@ gen.add("alternative_time_cost",   bool_t,   0,
 
 # Homotopy Class Planner
 
-gen.add("enable_homotopy_class_planning",    bool_t,    0,
-	"Activate homotopy class planning (Requires much more resources that simple planning, since multiple trajectory are optimized at once)", 
-	True)
-
 gen.add("enable_multithreading",    bool_t,    0,
 	"Activate multiple threading for planning multiple trajectories in parallel", 
 	True)
@@ -205,7 +201,7 @@ gen.add("obstacle_keypoint_offset", double_t, 0,
 
 gen.add("obstacle_heading_threshold", double_t, 0, 
 	"Specify the value of the scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration)", 
-	0, -1, 0.8) 
+	0.8, 0, 1) 
 
 gen.add("visualize_hc_graph",    bool_t,    0,
 	"Visualize the graph that is created for exploring new homotopy classes", 

diff --git a/cfg/rviz_test_optim.rviz b/cfg/rviz_test_optim.rviz
@@ -52,13 +52,7 @@ Visualization Manager:
       Class: rviz/Path
       Color: 25; 255; 0
       Enabled: true
-      Line Style: Lines
-      Line Width: 0.03
       Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
       Topic: /test_optim_node/local_plan
       Value: true
     - Arrow Length: 0.3
@@ -73,7 +67,7 @@ Visualization Manager:
       Marker Topic: /test_optim_node/teb_markers
       Name: Marker
       Namespaces:
-        PointObstacles: true
+        {}
       Queue Size: 100
       Value: true
     - Class: rviz/InteractiveMarkers
@@ -109,7 +103,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 9.41082
+      Distance: 11.8049
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1

diff --git a/launch/test_optim_node.launch b/launch/test_optim_node.launch
@@ -0,0 +1,12 @@
+<launch>
+
+  	<!--- Run optimization test node -->
+   	<node pkg="teb_local_planner" type="test_optim_node" name="test_optim_node" output="screen">
+	</node>
+
+  	<!-- RVIZ -->
+  	<node pkg="rviz" type="rviz" name="rviz" args="-d $(find teb_local_planner)/cfg/rviz_test_optim.rviz" />
+
+
+</launch>
+
diff --git a/src/teb_config.cpp b/src/teb_config.cpp
@@ -160,7 +160,6 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
   optim.alternative_time_cost = cfg.alternative_time_cost;
 
   // Homotopy Class Planner
-  hcp.enable_homotopy_class_planning = cfg.enable_homotopy_class_planning;
   hcp.enable_multithreading = cfg.enable_multithreading;
   hcp.simple_exploration = cfg.simple_exploration;
   hcp.max_number_classes = cfg.max_number_classes; 

diff --git a/src/test_optim_node.cpp b/src/test_optim_node.cpp
@@ -48,15 +48,17 @@
 using namespace teb_local_planner; // it is ok here to import everything for testing purposes
 
 // ============= Global Variables ================
+// Ok global variables are bad, but here we only have a simple testing node.
 PlannerInterfacePtr planner;
 TebVisualizationPtr visual;
 std::vector<ObstaclePtr> obst_vector;
 TebConfig config;
-
+boost::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg;
 
 // =========== Function declarations =============
 void CB_mainCycle(const ros::TimerEvent& e);
 void CB_publishCycle(const ros::TimerEvent& e);
+void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level);
 void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb);
 void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
 
@@ -75,6 +77,10 @@ int main( int argc, char** argv )
   ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle);
   ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle);
 
+  // setup dynamic reconfigure
+  dynamic_recfg = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(n);
+  dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(CB_reconfigure, _1, _2);
+  dynamic_recfg->setCallback(cb);
 
   // interactive marker server for simulated dynamic obstacles
   interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles");
@@ -137,6 +143,10 @@ void CB_publishCycle(const ros::TimerEvent& e)
   visual->publishObstacles(obst_vector);
 }
 
+void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level)
+{
+  config.reconfigure(reconfig);
+}
 
 void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb)
 {
-Original file line number
+Diff line change
@@ -0,0 +1,3 @@
+    .svn
+    .kdev4
+    *.kdev4