Dynamic reconfigure added to pose_controller ros side.

mutex guards enabled, and tested.
yujinrobot · Jan 16, 2017 · 5dfdfed · 5dfdfed
1 parent 29f6202
commit 5dfdfed
Show file tree

Hide file tree

Showing 6 changed files with 55 additions and 3 deletions.
diff --git a/yocs_diff_drive_pose_controller/CMakeLists.txt b/yocs_diff_drive_pose_controller/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(yocs_diff_drive_pose_controller)
 
 find_package(catkin REQUIRED COMPONENTS ecl_threads
+                                        dynamic_reconfigure
                                         geometry_msgs
                                         nodelet
                                         pluginlib
@@ -11,11 +12,13 @@ find_package(catkin REQUIRED COMPONENTS ecl_threads
                                         tf
                                         yocs_controllers
                                         yocs_math_toolkit
+                                        yocs_msgs
             )
 
 catkin_package(INCLUDE_DIRS include
                LIBRARIES yocs_diff_drive_pose_controller yocs_diff_drive_pose_controller_nodelet
                CATKIN_DEPENDS ecl_threads
+                              dynamic_reconfigure
                               geometry_msgs
                               nodelet
                               pluginlib
@@ -25,6 +28,7 @@ catkin_package(INCLUDE_DIRS include
                               tf
                               yocs_controllers
                               yocs_math_toolkit
+                              yocs_msgs
               )
 
 include_directories(include ${catkin_INCLUDE_DIRS})

diff --git a/...ve_pose_controller/include/yocs_diff_drive_pose_controller/diff_drive_pose_controller.hpp b/...ve_pose_controller/include/yocs_diff_drive_pose_controller/diff_drive_pose_controller.hpp
@@ -3,6 +3,7 @@
 
 #include <string>
 #include <yocs_controllers/default_controller.hpp>
+#include <ecl/threads/mutex.hpp>
 
 namespace yocs
 {
@@ -171,6 +172,8 @@ class DiffDrivePoseController : public Controller
   double orient_eps_;
   /// Enable or disable ros messages.
   bool verbose_;
+
+  ecl::Mutex controller_mutex_;
 };
 
 } /* end namespace */

diff --git a/...ose_controller/include/yocs_diff_drive_pose_controller/diff_drive_pose_controller_ros.hpp b/...ose_controller/include/yocs_diff_drive_pose_controller/diff_drive_pose_controller_ros.hpp
@@ -38,10 +38,14 @@
  ** Includes
  *****************************************************************************/
 #include <ros/ros.h>
+#include <ecl/threads/mutex.hpp>
 #include <std_msgs/Empty.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/String.h>
 #include <tf/transform_listener.h>
+#include <boost/shared_ptr.hpp>
+#include <dynamic_reconfigure/server.h>
+#include <yocs_msgs/PoseControllerConfig.h>
 
 #include "yocs_diff_drive_pose_controller/diff_drive_pose_controller.hpp"
 
@@ -132,6 +136,8 @@ class DiffDrivePoseControllerROS : public DiffDrivePoseController
    */
   void disableCB(const std_msgs::EmptyConstPtr msg);
 
+  void reconfigCB(yocs_msgs::PoseControllerConfig &config, uint32_t level);
+
   // basics
   ros::NodeHandle nh_;
   std::string name_;
@@ -156,6 +162,10 @@ class DiffDrivePoseControllerROS : public DiffDrivePoseController
   std::string base_frame_name_;
   /// frame name of the goal (pose)
   std::string goal_frame_name_;
+
+  ///dynamic reconfigure server
+  boost::shared_ptr<dynamic_reconfigure::Server<yocs_msgs::PoseControllerConfig> > reconfig_server_;
+
 };
 
 } // namespace yocs

diff --git a/yocs_diff_drive_pose_controller/package.xml b/yocs_diff_drive_pose_controller/package.xml
@@ -12,10 +12,11 @@
   <url type="website">http://ros.org/wiki/yocs_diff_drive_pose_controller</url>
   <url type="repository">https://github.com/yujinrobot/yujin_ocs</url>
   <url type="bugtracker">https://github.com/yujinrobot/yujin_ocs/issues</url>
-  
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>ecl_threads</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>pluginlib</build_depend>
@@ -25,8 +26,11 @@
   <build_depend>tf</build_depend>
   <build_depend>yocs_controllers</build_depend>
   <build_depend>yocs_math_toolkit</build_depend>
+  <build_depend>yocs_msgs</build_depend>
+
 
   <run_depend>ecl_threads</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>pluginlib</run_depend>
@@ -36,7 +40,8 @@
   <run_depend>tf</run_depend>
   <run_depend>yocs_controllers</run_depend>
   <run_depend>yocs_math_toolkit</run_depend>
-
+  <run_depend>yocs_msgs</run_depend>
+
   <export>
     <nodelet plugin="${prefix}/plugins/nodelets.xml"/>
   </export>

diff --git a/yocs_diff_drive_pose_controller/src/diff_drive_pose_controller.cpp b/yocs_diff_drive_pose_controller/src/diff_drive_pose_controller.cpp
@@ -62,6 +62,7 @@ bool DiffDrivePoseController::step()
 
 void DiffDrivePoseController::calculateControls()
 {
+  controller_mutex_.lock();
   double angle_diff = mtk::wrapAngle(theta_ - delta_);
 
   if (r_ > dist_thres_)
@@ -97,6 +98,7 @@ void DiffDrivePoseController::calculateControls()
       }
     }
   }
+  controller_mutex_.unlock();
 }
 
 void DiffDrivePoseController::controlPose()
@@ -126,7 +128,6 @@ void DiffDrivePoseController::controlPose()
 
 void DiffDrivePoseController::controlOrientation(double angle_difference)
 {
-
   w_ = orientation_gain_ * (angle_difference);
 
   //enforce limits on angular velocity

diff --git a/yocs_diff_drive_pose_controller/src/diff_drive_pose_controller_ros.cpp b/yocs_diff_drive_pose_controller/src/diff_drive_pose_controller_ros.cpp
@@ -133,6 +133,17 @@ bool DiffDrivePoseControllerROS::init()
       "base_frame_name = " << base_frame_name_ <<", goal_frame_name = " << goal_frame_name_ << " [" << name_ <<"]");
   ROS_DEBUG_STREAM(
       "v_max = " << v_max_ <<", k_1 = " << k_1_ << ", k_2 = " << k_2_ << ", beta = " << beta_ << ", lambda = " << lambda_ << ", dist_thres = " << dist_thres_ << ", orient_thres = " << orient_thres_ <<" [" << name_ <<"]");
+
+  reconfig_server_ = boost::shared_ptr<dynamic_reconfigure::Server<yocs_msgs::PoseControllerConfig> >(
+                               new dynamic_reconfigure::Server<yocs_msgs::PoseControllerConfig>(nh_));
+
+  ///dynamic reconfigure server callback type
+  dynamic_reconfigure::Server<yocs_msgs::PoseControllerConfig>::CallbackType reconfig_callback_func;
+
+  reconfig_callback_func = boost::bind(&DiffDrivePoseControllerROS::reconfigCB, this, _1, _2);
+
+  reconfig_server_->setCallback(reconfig_callback_func);
+
   return true;
 }
 
@@ -248,4 +259,22 @@ void DiffDrivePoseControllerROS::disableCB(const std_msgs::EmptyConstPtr msg)
   }
 }
 
+void DiffDrivePoseControllerROS::reconfigCB(yocs_msgs::PoseControllerConfig &config, uint32_t level)
+{
+  controller_mutex_.lock();
+  v_min_movement_ = config.v_min;
+  v_max_ = config.v_max;
+  w_max_ = config.w_max;
+  w_min_ = config.w_min;
+  k_1_ = config.k_1;
+  k_2_ = config.k_2;
+  beta_ = config.beta;
+  lambda_ = config.lambda;
+  dist_thres_ = config.dist_thres;
+  orient_thres_ = config.orient_thres;
+  dist_eps_ = config.dist_eps;
+  orient_eps_ = config.orient_eps;
+  controller_mutex_.unlock();
+}
+
 } // namespace yocs