From 772137dd6d1bfeb5cd9657d72e17ca8c66d42b99 Mon Sep 17 00:00:00 2001 From: vrabaud Date: Tue, 18 Sep 2012 10:20:05 +0000 Subject: [PATCH 1/3] Tagging pr2_controllers-1.9.0 new release From b4ce6cb0701c0ffe902145778a989731c6ff1554 Mon Sep 17 00:00:00 2001 From: furushchev Date: Sat, 4 Jan 2014 19:33:24 +0900 Subject: [PATCH 2/3] add feedback publisher of follow_joint_trajectory --- .../joint_trajectory_action_controller.h | 8 +++-- .../joint_trajectory_action_controller.cpp | 30 +++++++++++++++++++ 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_trajectory_action_controller.h b/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_trajectory_action_controller.h index 869e911..27a7ccb 100644 --- a/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_trajectory_action_controller.h +++ b/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_trajectory_action_controller.h @@ -71,6 +71,7 @@ class RTServerGoalHandle //typedef actionlib::ActionServer::GoalHandle GoalHandle; typedef actionlib::ServerGoalHandle GoalHandle; typedef boost::shared_ptr ResultPtr; + typedef boost::shared_ptr FeedbackPtr; uint8_t state_; @@ -81,12 +82,15 @@ class RTServerGoalHandle public: GoalHandle gh_; ResultPtr preallocated_result_; // Preallocated so it can be used in realtime + FeedbackPtr preallocated_feedback_; - RTServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL)) - : req_abort_(false), req_succeed_(false), gh_(gh), preallocated_result_(preallocated_result) + RTServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL)) + : req_abort_(false), req_succeed_(false), gh_(gh), preallocated_result_(preallocated_result) { if (!preallocated_result_) preallocated_result_.reset(new Result); + if (!preallocated_feedback_) + preallocated_feedback_.reset(new Feedback); } void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL)) diff --git a/robot_mechanism_controllers/src/joint_trajectory_action_controller.cpp b/robot_mechanism_controllers/src/joint_trajectory_action_controller.cpp index ba4fb7b..698bf83 100644 --- a/robot_mechanism_controllers/src/joint_trajectory_action_controller.cpp +++ b/robot_mechanism_controllers/src/joint_trajectory_action_controller.cpp @@ -492,6 +492,36 @@ void JointTrajectoryActionController::update() } controller_state_publisher_->unlockAndPublish(); } + if (current_active_goal_follow) + { + current_active_goal_follow->preallocated_feedback_->header.stamp = time; + current_active_goal_follow->preallocated_feedback_->joint_names.resize(joints_.size()); + current_active_goal_follow->preallocated_feedback_->desired.positions.resize(joints_.size()); + current_active_goal_follow->preallocated_feedback_->desired.velocities.resize(joints_.size()); + current_active_goal_follow->preallocated_feedback_->desired.accelerations.resize(joints_.size()); + current_active_goal_follow->preallocated_feedback_->actual.positions.resize(joints_.size()); + current_active_goal_follow->preallocated_feedback_->actual.velocities.resize(joints_.size()); + current_active_goal_follow->preallocated_feedback_->error.positions.resize(joints_.size()); + current_active_goal_follow->preallocated_feedback_->error.velocities.resize(joints_.size()); + for (size_t j = 0; j < joints_.size(); ++j) + { + current_active_goal_follow->preallocated_feedback_->joint_names[j] = joints_[j]->joint_->name; + current_active_goal_follow->preallocated_feedback_->desired.positions[j] = q[j]; + current_active_goal_follow->preallocated_feedback_->desired.velocities[j] = qd[j]; + current_active_goal_follow->preallocated_feedback_->desired.accelerations[j] = qdd[j]; + current_active_goal_follow->preallocated_feedback_->actual.positions[j] = joints_[j]->position_; + current_active_goal_follow->preallocated_feedback_->actual.velocities[j] = joints_[j]->velocity_; + current_active_goal_follow->preallocated_feedback_->error.positions[j] = error[j]; + current_active_goal_follow->preallocated_feedback_->error.velocities[j] = joints_[j]->velocity_ - qd[j]; + } + const actionlib_msgs::GoalID goalID = current_active_goal_follow->gh_.getGoalID(); + ros::Duration time_from_start = time - goalID.stamp; + current_active_goal_follow->preallocated_feedback_->desired.time_from_start = time_from_start; + current_active_goal_follow->preallocated_feedback_->actual.time_from_start = time_from_start; + current_active_goal_follow->preallocated_feedback_->error.time_from_start = time_from_start; + current_active_goal_follow->gh_.publishFeedback(*current_active_goal_follow->preallocated_feedback_); + + } } ++loop_count_; From c3e4722c755467a7f0cbdbe4fc7fa76538a6faac Mon Sep 17 00:00:00 2001 From: Gustavo Goretkin Date: Tue, 17 Mar 2015 10:02:45 -0700 Subject: [PATCH 3/3] 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 27fc9a5..05f3e0a 100644 --- a/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h +++ b/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h @@ -114,7 +114,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 4f2bd89..588e330 100644 --- 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(command_, joint_state_->position_, joint_state_->joint_->limits->lower, joint_state_->joint_->limits->upper,error);