From 5548003fc231947de7dac3808065e076f5b4279e Mon Sep 17 00:00:00 2001 From: Gustavo Goretkin Date: Tue, 17 Mar 2015 10:02:45 -0700 Subject: [PATCH] remove redundant/incorrect code. there is no need for an `initialized_` flag, because the controller manager will call `starting` to indicate the first iteration of a control cycle. The controller already does the correct thing in `starting`. If instead the code in `starting` were removed, and the `initialized_` flag kept, then this position controller would (incorrectly) jump to the previous set point if it is stopped and started (for example, if you hit and release the run-stop), since the `initialized_` flag was only being cleared in the constructor. --- .../joint_position_controller.h | 1 - .../src/joint_position_controller.cpp | 8 +------- 2 files changed, 1 insertion(+), 8 deletions(-) diff --git a/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h b/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h index 65bf00b..6833d8c 100755 --- a/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h +++ b/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h @@ -113,7 +113,6 @@ class JointPositionController : public pr2_controller_interface::Controller private: int loop_count_; - bool initialized_; pr2_mechanism_model::RobotState *robot_; /**< Pointer to robot structure. */ control_toolbox::Pid pid_controller_; /**< Internal PID controller. */ ros::Time last_time_; /**< Last time stamp of update. */ diff --git a/robot_mechanism_controllers/src/joint_position_controller.cpp b/robot_mechanism_controllers/src/joint_position_controller.cpp index 3aee240..48b41f7 100755 --- a/robot_mechanism_controllers/src/joint_position_controller.cpp +++ b/robot_mechanism_controllers/src/joint_position_controller.cpp @@ -44,7 +44,7 @@ namespace controller { JointPositionController::JointPositionController() : joint_state_(NULL), command_(0), - loop_count_(0), initialized_(false), robot_(NULL), last_time_(0) + loop_count_(0), robot_(NULL), last_time_(0) { } @@ -141,12 +141,6 @@ void JointPositionController::update() assert(joint_state_->joint_); dt_= time - last_time_; - if (!initialized_) - { - initialized_ = true; - command_ = joint_state_->position_; - } - if(joint_state_->joint_->type == urdf::Joint::REVOLUTE) { angles::shortest_angular_distance_with_limits(