corrected mutex logic.

yujinrobot · Nov 28, 2016 · a4b8a81 · a4b8a81
1 parent 583826a
commit a4b8a81
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 17 deletions.
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
@@ -166,13 +166,6 @@ class DiffDrivePoseControllerROS : public DiffDrivePoseController
   ///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

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
@@ -103,7 +103,6 @@ void DiffDrivePoseController::calculateControls()
 
 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_));
@@ -125,20 +124,15 @@ 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)

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
@@ -136,8 +136,13 @@ bool DiffDrivePoseControllerROS::init()
 
   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_);
+
+  ///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;
 }
@@ -256,7 +261,7 @@ void DiffDrivePoseControllerROS::disableCB(const std_msgs::EmptyConstPtr msg)
 
 void DiffDrivePoseControllerROS::reconfigCB(yocs_msgs::PoseControllerConfig &config, uint32_t level)
 {
-  dynamic_reconfig_mutex_.lock();
+  controller_mutex_.lock();
   v_min_movement_ = config.v_min;
   v_max_ = config.v_max;
   w_max_ = config.w_max;
@@ -269,7 +274,7 @@ void DiffDrivePoseControllerROS::reconfigCB(yocs_msgs::PoseControllerConfig &con
   orient_thres_ = config.orient_thres;
   dist_eps_ = config.dist_eps;
   orient_eps_ = config.orient_eps;
-  dynamic_reconfig_mutex_.unlock();
+  controller_mutex_.unlock();
 }
 
 } // namespace yocs