reenabled holonomic drive control. still untested since groovy/hydro …

…migration. closes #2
utexas-bwi · Aug 9, 2013 · b12745e · b12745e
1 parent ab529a2
commit b12745e
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 5 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -5,6 +5,7 @@ project(eband_local_planner)
 find_package(catkin REQUIRED 
   COMPONENTS
     base_local_planner
+    control_toolbox
     costmap_2d
     geometry_msgs
     nav_core
@@ -25,6 +26,7 @@ include_directories(
 catkin_package(
   CATKIN_DEPENDS
     base_local_planner
+    control_toolbox
     costmap_2d
     geometry_msgs
     nav_core

diff --git a/include/eband_local_planner/eband_trajectory_controller.h b/include/eband_local_planner/eband_trajectory_controller.h
@@ -58,6 +58,9 @@
 #include <angles/angles.h>
 #include <tf/tf.h>
 
+// PID control library
+#include <control_toolbox/pid.h>
+
 namespace eband_local_planner{
 
 /**
@@ -123,6 +126,8 @@ class EBandTrajectoryCtrl{
 		costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap
 		boost::shared_ptr<EBandVisualization> target_visual_; // pointer to visualization object
 
+    control_toolbox::Pid pid_;
+
 		// parameters
     bool differential_drive_hack_;
     double k_p_, k_nu_, k_int_, k_diff_, ctrl_freq_;

diff --git a/package.xml b/package.xml
@@ -18,6 +18,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>base_local_planner</build_depend>
+  <build_depend>control_toolbox</build_depend>
   <build_depend>costmap_2d</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nav_core</build_depend>
@@ -28,6 +29,7 @@
   <build_depend>tf_conversions</build_depend>
 
   <run_depend>base_local_planner</run_depend>
+  <run_depend>control_toolbox</run_depend>
   <run_depend>costmap_2d</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>nav_core</run_depend>

diff --git a/src/eband_trajectory_controller.cpp b/src/eband_trajectory_controller.cpp
@@ -55,8 +55,7 @@ EBandTrajectoryCtrl::EBandTrajectoryCtrl(std::string name, costmap_2d::Costmap2D
 	initialize(name, costmap_ros);
 
   // Initialize pid object (note we'll be further clamping its output)
-  // TODO See #1, #2
-  //pid_.initPid(1, 0, 0, 10, -10);
+  pid_.initPid(1, 0, 0, 10, -10);
 }
 
 
@@ -537,9 +536,7 @@ bool EBandTrajectoryCtrl::getTwist(geometry_msgs::Twist& twist_cmd, bool& goal_r
         {
 
           const double angular_diff = angularDiff(control_deviation, elastic_band_.at(0).center.pose);
-          // TODO See Issue #1, #2
-          //const double vel = pid_.updatePid(-angular_diff, ros::Duration(1/ctrl_freq_));
-          const double vel = 0;
+          const double vel = pid_.computeCommand(angular_diff, ros::Duration(1/ctrl_freq_));
           const double mult = fabs(vel) > max_vel_th_ ? max_vel_th_/fabs(vel) : 1.0;
           control_deviation.angular.z = vel*mult;
           const double abs_vel = fabs(control_deviation.angular.z);