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_effort_controller.h"
00036 #include "pluginlib/class_list_macros.h"
00037
00038 PLUGINLIB_EXPORT_CLASS( controller::JointEffortController, pr2_controller_interface::Controller)
00039
00040 namespace controller {
00041
00042 JointEffortController::JointEffortController()
00043 : joint_state_(NULL), command_(0), robot_(NULL)
00044 {
00045 }
00046
00047 JointEffortController::~JointEffortController()
00048 {
00049 sub_command_.shutdown();
00050 }
00051
00052 bool JointEffortController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name)
00053 {
00054 if (!robot)
00055 {
00056 ROS_ERROR("The given robot was NULL");
00057 return false;
00058 }
00059 robot_ = robot;
00060
00061 joint_state_ = robot_->getJointState(joint_name);
00062 if (!joint_state_)
00063 {
00064 ROS_ERROR("JointEffortController could not find joint named \"%s\"",
00065 joint_name.c_str());
00066 return false;
00067 }
00068
00069 return true;
00070 }
00071
00072 bool JointEffortController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00073 {
00074 assert(robot);
00075 node_ = n;
00076 robot_ = robot;
00077
00078 std::string joint_name;
00079 if (!node_.getParam("joint", joint_name))
00080 {
00081 ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00082 return false;
00083 }
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 sub_command_ = node_.subscribe<std_msgs::Float64>(
00093 "command", 1, &JointEffortController::commandCB, this);
00094
00095 return true;
00096 }
00097
00098 void JointEffortController::update()
00099 {
00100 joint_state_->commanded_effort_ += command_;
00101 }
00102
00103 void JointEffortController::commandCB(const std_msgs::Float64ConstPtr& msg)
00104 {
00105 command_ = msg->data;
00106 }
00107
00108 }