Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "pr2_mechanism_controllers/pr2_gripper_controller.h"
00036 #include "angles/angles.h"
00037 #include "pluginlib/class_list_macros.h"
00038
00039 PLUGINLIB_EXPORT_CLASS( controller::Pr2GripperController, pr2_controller_interface::Controller)
00040
00041 using namespace std;
00042
00043 namespace controller {
00044
00045 Pr2GripperController::Pr2GripperController()
00046 : joint_state_(NULL),
00047 loop_count_(0), robot_(NULL), last_time_(0)
00048 {
00049 }
00050
00051 Pr2GripperController::~Pr2GripperController()
00052 {
00053 sub_command_.shutdown();
00054 }
00055
00056 bool Pr2GripperController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00057 {
00058 assert(robot);
00059 node_ = n;
00060 robot_ = robot;
00061
00062 std::string joint_name;
00063 if (!node_.getParam("joint", joint_name)) {
00064 ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00065 return false;
00066 }
00067 if (!(joint_state_ = robot_->getJointState(joint_name)))
00068 {
00069 ROS_ERROR("Could not find joint named \"%s\" (namespace: %s)",
00070 joint_name.c_str(), node_.getNamespace().c_str());
00071 return false;
00072 }
00073 if (joint_state_->joint_->type != urdf::Joint::PRISMATIC)
00074 {
00075 ROS_ERROR("The joint \"%s\" was not prismatic (namespace: %s)",
00076 joint_name.c_str(), node_.getNamespace().c_str());
00077 return false;
00078 }
00079
00080 if (!joint_state_->calibrated_)
00081 {
00082 ROS_ERROR("Joint %s is not calibrated (namespace: %s)",
00083 joint_state_->joint_->name.c_str(), node_.getNamespace().c_str());
00084 return false;
00085 }
00086
00087 if (!pid_.init(ros::NodeHandle(node_, "pid")))
00088 return false;
00089
00090 controller_state_publisher_.reset(
00091 new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointControllerState>
00092 (node_, "state", 1));
00093
00094 sub_command_ = node_.subscribe<pr2_controllers_msgs::Pr2GripperCommand>(
00095 "command", 1, &Pr2GripperController::commandCB, this);
00096
00097 return true;
00098 }
00099
00100 void Pr2GripperController::update()
00101 {
00102 if (!joint_state_->calibrated_)
00103 return;
00104
00105 assert(robot_ != NULL);
00106 double error(0);
00107 ros::Time time = robot_->getTime();
00108 assert(joint_state_->joint_);
00109 ros::Duration dt = time - last_time_;
00110
00111 pr2_controllers_msgs::Pr2GripperCommandConstPtr command;
00112 command_box_.get(command);
00113 assert(command);
00114
00115
00116 error = joint_state_->position_ - command->position;
00117
00118
00119 double effort = pid_.updatePid(error, joint_state_->velocity_, dt);
00120 if (command->max_effort >= 0.0)
00121 {
00122 effort = std::max(-command->max_effort, std::min(effort, command->max_effort));
00123 }
00124 joint_state_->commanded_effort_ = effort;
00125
00126 if(loop_count_ % 10 == 0)
00127 {
00128 if(controller_state_publisher_ && controller_state_publisher_->trylock())
00129 {
00130 controller_state_publisher_->msg_.header.stamp = time;
00131 controller_state_publisher_->msg_.set_point = command->position;
00132 controller_state_publisher_->msg_.process_value = joint_state_->position_;
00133 controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
00134 controller_state_publisher_->msg_.error = error;
00135 controller_state_publisher_->msg_.time_step = dt.toSec();
00136 controller_state_publisher_->msg_.command = effort;
00137
00138 double dummy;
00139 pid_.getGains(controller_state_publisher_->msg_.p,
00140 controller_state_publisher_->msg_.i,
00141 controller_state_publisher_->msg_.d,
00142 controller_state_publisher_->msg_.i_clamp,
00143 dummy);
00144 controller_state_publisher_->unlockAndPublish();
00145 }
00146 }
00147 loop_count_++;
00148
00149 last_time_ = time;
00150 }
00151
00152 void Pr2GripperController::commandCB(const pr2_controllers_msgs::Pr2GripperCommandConstPtr& msg)
00153 {
00154 command_box_.set(msg);
00155 }
00156
00157 }