added mutex ro dynamic reconfig parameters.

Author: Marco-Andarcia

yocs_diff_drive_pose_controller/include/yocs_diff_drive_pose_controller/diff_drive_pose_controller.hpp

@@ -3,7 +3,7 @@
 
 #include <string>
 #include <yocs_controllers/default_controller.hpp>
-
+#include <ecl/threads/mutex.hpp>
 namespace yocs
 {
 
@@ -171,6 +171,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/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

@@ -41,7 +41,8 @@ bool DiffDrivePoseControllerROS::init()
 {
   enable_controller_subscriber_ = nh_.subscribe("enable", 10, &DiffDrivePoseControllerROS::enableCB, this);
   disable_controller_subscriber_ = nh_.subscribe("disable", 10, &DiffDrivePoseControllerROS::disableCB, this);
-  control_velocity_subscriber_ = nh_.subscribe("control_max_vel", 10, &DiffDrivePoseControllerROS::controlMaxVelCB,this);
+  control_velocity_subscriber_ = nh_.subscribe("control_max_vel", 10, &DiffDrivePoseControllerROS::controlMaxVelCB,
+                                               this);
   command_velocity_publisher_ = nh_.advertise<geometry_msgs::Twist>("command_velocity", 10);
   pose_reached_publisher_ = nh_.advertise<std_msgs::Bool>("pose_reached", 10);
 
@@ -134,10 +135,9 @@ bool DiffDrivePoseControllerROS::init()
       "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<gopher_navi_msgs::PoseControllerConfig> >(
-        new dynamic_reconfigure::Server<gopher_navi_msgs::PoseControllerConfig>(nh_));
-    reconfig_callback_func_ = boost::bind(&DiffDrivePoseControllerROS::reconfigCB, this, _1, _2);
-    reconfig_server_->setCallback(reconfig_callback_func_);
-
+                               new dynamic_reconfigure::Server<gopher_navi_msgs::PoseControllerConfig>(nh_));
+  reconfig_callback_func_ = boost::bind(&DiffDrivePoseControllerROS::reconfigCB, this, _1, _2);
+  reconfig_server_->setCallback(reconfig_callback_func_);
 
   return true;
 }