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 "robot_mechanism_controllers/joint_velocity_controller.h"
00036 #include "pluginlib/class_list_macros.h"
00037
00038 PLUGINLIB_EXPORT_CLASS( controller::JointVelocityController, pr2_controller_interface::Controller)
00039
00040 using namespace std;
00041
00042 namespace controller {
00043
00044 JointVelocityController::JointVelocityController()
00045 : joint_state_(NULL), command_(0), robot_(NULL), last_time_(0), loop_count_(0)
00046 {
00047 }
00048
00049 JointVelocityController::~JointVelocityController()
00050 {
00051 sub_command_.shutdown();
00052 }
00053
00054 bool JointVelocityController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name,
00055 const control_toolbox::Pid &pid)
00056 {
00057 assert(robot);
00058 robot_ = robot;
00059 last_time_ = robot->getTime();
00060
00061 joint_state_ = robot_->getJointState(joint_name);
00062 if (!joint_state_)
00063 {
00064 ROS_ERROR("JointVelocityController could not find joint named \"%s\"\n",
00065 joint_name.c_str());
00066 return false;
00067 }
00068
00069 pid_controller_ = pid;
00070
00071 return true;
00072 }
00073
00074 bool JointVelocityController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00075 {
00076 assert(robot);
00077 node_ = n;
00078 robot_ = robot;
00079
00080 std::string joint_name;
00081 if (!node_.getParam("joint", joint_name)) {
00082 ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00083 return false;
00084 }
00085 if (!(joint_state_ = robot->getJointState(joint_name)))
00086 {
00087 ROS_ERROR("Could not find joint \"%s\" (namespace: %s)",
00088 joint_name.c_str(), node_.getNamespace().c_str());
00089 return false;
00090 }
00091
00092 if (!pid_controller_.init(ros::NodeHandle(node_, "pid")))
00093 return false;
00094
00095 controller_state_publisher_.reset(
00096 new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointControllerState>
00097 (node_, "state", 1));
00098
00099 sub_command_ = node_.subscribe<std_msgs::Float64>("command", 1, &JointVelocityController::setCommandCB, this);
00100
00101 return true;
00102 }
00103
00104
00105 void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
00106 {
00107 pid_controller_.setGains(p,i,d,i_max,i_min);
00108
00109 }
00110
00111 void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00112 {
00113 pid_controller_.getGains(p,i,d,i_max,i_min);
00114 }
00115
00116 std::string JointVelocityController::getJointName()
00117 {
00118 return joint_state_->joint_->name;
00119 }
00120
00121
00122 void JointVelocityController::setCommand(double cmd)
00123 {
00124 command_ = cmd;
00125 }
00126
00127
00128 void JointVelocityController::getCommand(double & cmd)
00129 {
00130 cmd = command_;
00131 }
00132
00133 void JointVelocityController::update()
00134 {
00135 assert(robot_ != NULL);
00136 ros::Time time = robot_->getTime();
00137
00138 double error = command_ - joint_state_->velocity_;
00139 dt_ = time - last_time_;
00140 double command = pid_controller_.computeCommand(error, dt_);
00141 joint_state_->commanded_effort_ += command;
00142
00143 if(loop_count_ % 10 == 0)
00144 {
00145 if(controller_state_publisher_ && controller_state_publisher_->trylock())
00146 {
00147 controller_state_publisher_->msg_.header.stamp = time;
00148 controller_state_publisher_->msg_.set_point = command_;
00149 controller_state_publisher_->msg_.process_value = joint_state_->velocity_;
00150 controller_state_publisher_->msg_.error = error;
00151 controller_state_publisher_->msg_.time_step = dt_.toSec();
00152 controller_state_publisher_->msg_.command = command;
00153
00154 double dummy;
00155 getGains(controller_state_publisher_->msg_.p,
00156 controller_state_publisher_->msg_.i,
00157 controller_state_publisher_->msg_.d,
00158 controller_state_publisher_->msg_.i_clamp,
00159 dummy);
00160 controller_state_publisher_->unlockAndPublish();
00161 }
00162 }
00163 loop_count_++;
00164
00165 last_time_ = time;
00166 }
00167
00168 void JointVelocityController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
00169 {
00170 command_ = msg->data;
00171 }
00172
00173 }