Additional locking, w_min bugfix

yujinrobot · Jan 16, 2017 · eb9ab14 · eb9ab14
1 parent 5dfdfed
commit eb9ab14
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 1 deletion.
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
@@ -48,10 +48,12 @@ void DiffDrivePoseController::setInput(double distance_to_goal, double delta, do
 
 void DiffDrivePoseController::setCurrentLimits(double v_min, double w_min, double v_max, double w_max)
 {
+  controller_mutex_.lock();
   v_min_ = v_min;
   w_min_ = w_min;
   v_max_ = v_max;
   w_max_ = w_max;
+  controller_mutex_.unlock();
 }
 
 bool DiffDrivePoseController::step()

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
@@ -225,9 +225,12 @@ void DiffDrivePoseControllerROS::setControlOutput()
 
 void DiffDrivePoseControllerROS::controlMaxVelCB(const std_msgs::Float32ConstPtr msg)
 {
+  controller_mutex_.lock();
   v_max_ = msg->data;
+
   //v_min_ = -v_max_; //if we also want to enable driving backwards
   ROS_INFO_STREAM("Maximum linear control velocity has been set to " << v_max_ << ". [" << name_ << "]");
+  controller_mutex_.unlock();
 }
 
 void DiffDrivePoseControllerROS::enableCB(const std_msgs::StringConstPtr msg)
@@ -263,9 +266,10 @@ void DiffDrivePoseControllerROS::reconfigCB(yocs_msgs::PoseControllerConfig &con
 {
   controller_mutex_.lock();
   v_min_movement_ = config.v_min;
+  v_min_ = v_min_movement_;
   v_max_ = config.v_max;
   w_max_ = config.w_max;
-  w_min_ = config.w_min;
+  w_min_ = -w_max_;
   k_1_ = config.k_1;
   k_2_ = config.k_2;
   beta_ = config.beta;