got the code to compile. still needs testing.

added mutex ro dynamic reconfig parameters.

added header for mutex.

now using yocs_msgs

Author: Marco-Andarcia

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})

yocs_diff_drive_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 */

yocs_diff_drive_pose_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,17 @@ 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_;
+
+  ///dynamic reconfigure server callback type
+  dynamic_reconfigure::Server<yocs_msgs::PoseControllerConfig>::CallbackType reconfig_callback_func_;
+
+  yocs_msgs::PoseControllerConfig test_;
+
+  ecl::Mutex dynamic_reconfig_mutex_;
+
 };
 
 } // namespace yocs

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>

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,10 +98,12 @@ void DiffDrivePoseController::calculateControls()
       }
     }
   }
+  controller_mutex_.unlock();
 }
 
 void DiffDrivePoseController::controlPose()
 {
+  controller_mutex_.lock();
   double atan2_k1_theta = std::atan2(-k_1_*theta_, 1.0);
   cur_ = (-1 / r_)
       * (k_2_ * (delta_ - atan2_k1_theta) + (1 + (k_1_ / (1 + std::pow((k_1_ * theta_), 2)))) * sin(delta_));
@@ -122,16 +125,20 @@ void DiffDrivePoseController::controlPose()
   v_ = enforceMinMax(v_, v_min_, v_max_);
 
 //  ROS_WARN_STREAM("r_: " << r_ << " dist_thres_: " << dist_thres_ << ", delta_-theta_: " << delta_ - theta_ << ", orient_thres_" << orient_thres_);
+  controller_mutex_.unlock();
 }
 
 void DiffDrivePoseController::controlOrientation(double angle_difference)
 {
+  controller_mutex_.lock();
 
   w_ = orientation_gain_ * (angle_difference);
 
   //enforce limits on angular velocity
   w_ = enforceMinVelocity(w_, w_min_movement_);
   w_ = enforceMinMax(w_, w_min_, w_max_);
+
+  controller_mutex_.unlock();
 }
 
 double DiffDrivePoseController::enforceMinMax(double& value, double min, double max)

yocs_diff_drive_pose_controller/src/diff_drive_pose_controller_ros.cpp

@@ -133,6 +133,12 @@ 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_));
+  reconfig_callback_func_ = boost::bind(&DiffDrivePoseControllerROS::reconfigCB, this, _1, _2);
+  reconfig_server_->setCallback(reconfig_callback_func_);
+
   return true;
 }
 
@@ -248,4 +254,22 @@ void DiffDrivePoseControllerROS::disableCB(const std_msgs::EmptyConstPtr msg)
   }
 }
 
+void DiffDrivePoseControllerROS::reconfigCB(yocs_msgs::PoseControllerConfig &config, uint32_t level)
+{
+  dynamic_reconfig_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;
+  dynamic_reconfig_mutex_.unlock();
+}
+
 } // namespace yocs