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 cc532673d2b119d9f8403da66f76097e7e97ea57 Mon Sep 17 00:00:00 2001 From: Gustavo Goretkin Date: Mon, 16 Mar 2015 21:08:41 -0700 Subject: [PATCH 3/3] publish state of PID integrator in i_error --- pr2_controllers_msgs/msg/JointControllerState.msg | 1 + robot_mechanism_controllers/src/joint_position_controller.cpp | 4 ++++ robot_mechanism_controllers/src/joint_velocity_controller.cpp | 4 ++++ 3 files changed, 9 insertions(+) diff --git a/pr2_controllers_msgs/msg/JointControllerState.msg b/pr2_controllers_msgs/msg/JointControllerState.msg index e2bceb1..5f57150 100644 --- a/pr2_controllers_msgs/msg/JointControllerState.msg +++ b/pr2_controllers_msgs/msg/JointControllerState.msg @@ -3,6 +3,7 @@ float64 set_point float64 process_value float64 process_value_dot float64 error +float64 i_error float64 time_step float64 command float64 p diff --git a/robot_mechanism_controllers/src/joint_position_controller.cpp b/robot_mechanism_controllers/src/joint_position_controller.cpp index 4f2bd89..6f2f1c0 100644 --- a/robot_mechanism_controllers/src/joint_position_controller.cpp +++ b/robot_mechanism_controllers/src/joint_position_controller.cpp @@ -169,11 +169,15 @@ void JointPositionController::update() { if(controller_state_publisher_ && controller_state_publisher_->trylock()) { + double p_error, i_error, d_error; + pid_controller_.getCurrentPIDErrors(&p_error, &i_error, &d_error); + controller_state_publisher_->msg_.header.stamp = time; controller_state_publisher_->msg_.set_point = command_; controller_state_publisher_->msg_.process_value = joint_state_->position_; controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_; controller_state_publisher_->msg_.error = error; + controller_state_publisher_->msg_.i_error = i_error; controller_state_publisher_->msg_.time_step = dt_.toSec(); controller_state_publisher_->msg_.command = commanded_effort; diff --git a/robot_mechanism_controllers/src/joint_velocity_controller.cpp b/robot_mechanism_controllers/src/joint_velocity_controller.cpp index fa5e934..4841f6e 100644 --- a/robot_mechanism_controllers/src/joint_velocity_controller.cpp +++ b/robot_mechanism_controllers/src/joint_velocity_controller.cpp @@ -144,10 +144,14 @@ void JointVelocityController::update() { if(controller_state_publisher_ && controller_state_publisher_->trylock()) { + double p_error, i_error, d_error; + pid_controller_.getCurrentPIDErrors(&p_error, &i_error, &d_error); + controller_state_publisher_->msg_.header.stamp = time; controller_state_publisher_->msg_.set_point = command_; controller_state_publisher_->msg_.process_value = joint_state_->velocity_; controller_state_publisher_->msg_.error = error; + controller_state_publisher_->msg_.i_error = i_error; controller_state_publisher_->msg_.time_step = dt_.toSec(); controller_state_publisher_->msg_.command = command;