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 #include "pr2_mechanism_controllers/caster_controller.h"
00035 #include "pluginlib/class_list_macros.h"
00036
00037 PLUGINLIB_EXPORT_CLASS( controller::CasterController, pr2_controller_interface::Controller)
00038
00039 namespace controller {
00040
00041 const double CasterController::WHEEL_RADIUS = 0.079;
00042 const double CasterController::WHEEL_OFFSET = 0.049;
00043
00044 CasterController::CasterController()
00045 : steer_velocity_(0), drive_velocity_(0)
00046 {
00047 }
00048
00049 CasterController::~CasterController()
00050 {
00051 }
00052
00053 bool CasterController::init(
00054 pr2_mechanism_model::RobotState *robot,
00055 const std::string &caster_joint,
00056 const std::string &wheel_l_joint, const std::string &wheel_r_joint,
00057 const control_toolbox::Pid &caster_pid, const control_toolbox::Pid &wheel_pid)
00058 {
00059 caster_ = robot->getJointState(caster_joint);
00060 if (!caster_)
00061 {
00062 fprintf(stderr, "Error: Caster joint \"%s\" does not exist\n", caster_joint.c_str());
00063 return false;
00064 }
00065
00066 if (!caster_vel_.init(robot, caster_joint, caster_pid))
00067 return false;
00068 if (!wheel_l_vel_.init(robot, wheel_l_joint, wheel_pid))
00069 return false;
00070 if (!wheel_r_vel_.init(robot, wheel_r_joint, wheel_pid))
00071 return false;
00072
00073 return true;
00074 }
00075
00076 bool CasterController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00077 {
00078 node_ = n;
00079 assert(robot);
00080
00081
00082
00083 std::string caster_joint_name, wheel_l_joint_name, wheel_r_joint_name;
00084 if (!node_.getParam("joints/caster", caster_joint_name))
00085 {
00086 ROS_ERROR("No caster joint given (namespace: %s)", node_.getNamespace().c_str());
00087 return false;
00088 }
00089 if (!node_.getParam("joints/wheel_l", wheel_l_joint_name))
00090 {
00091 ROS_ERROR("No wheel_l joint given (namespace: %s)", node_.getNamespace().c_str());
00092 return false;
00093 }
00094 if (!node_.getParam("joints/wheel_r", wheel_r_joint_name))
00095 {
00096 ROS_ERROR("No wheel_r joint given (namespace: %s)", node_.getNamespace().c_str());
00097 return false;
00098 }
00099 if (!(caster_ = robot->getJointState(caster_joint_name)))
00100 {
00101 ROS_ERROR("Caster joint \"%s\" does not exist (namespace: %s)",
00102 caster_joint_name.c_str(), node_.getNamespace().c_str());
00103 return false;
00104 }
00105
00106
00107
00108 XmlRpc::XmlRpcValue caster_pid, wheel_pid;
00109 node_.getParam("caster_pid", caster_pid);
00110 node_.getParam("wheel_pid", wheel_pid);
00111
00112 ros::NodeHandle
00113 caster_node(n, "caster"),
00114 wheel_l_node(n, "wheel_l"),
00115 wheel_r_node(n, "wheel_r");
00116
00117 caster_node.setParam("type", std::string("JointVelocityController"));
00118 caster_node.setParam("joint", caster_joint_name);
00119 caster_node.setParam("pid", caster_pid);
00120
00121 wheel_l_node.setParam("type", std::string("JointVelocityController"));
00122 wheel_l_node.setParam("joint", wheel_l_joint_name);
00123 wheel_l_node.setParam("pid", wheel_pid);
00124
00125 wheel_r_node.setParam("type", std::string("JointVelocityController"));
00126 wheel_r_node.setParam("joint", wheel_r_joint_name);
00127 wheel_r_node.setParam("pid", wheel_pid);
00128
00129 assert(robot);
00130 if (!caster_vel_.init(robot, caster_node)) return false;
00131 if (!wheel_l_vel_.init(robot, wheel_l_node)) return false;
00132 if (!wheel_r_vel_.init(robot, wheel_r_node)) return false;
00133
00134 steer_cmd_ = node_.subscribe<std_msgs::Float64>("steer", 1, &CasterController::setSteerCB, this);
00135 drive_cmd_ = node_.subscribe<std_msgs::Float64>("drive", 1, &CasterController::setDriveCB, this);
00136
00137 return true;
00138 }
00139
00140 void CasterController::update()
00141 {
00142 caster_vel_.setCommand(steer_velocity_);
00143
00144 double wd = drive_velocity_ / WHEEL_RADIUS;
00145 double ws = (WHEEL_OFFSET / WHEEL_RADIUS) * steer_velocity_;
00146 wheel_r_vel_.setCommand(wd + ws);
00147 wheel_l_vel_.setCommand(wd - ws);
00148
00149 caster_vel_.update();
00150 wheel_l_vel_.update();
00151 wheel_r_vel_.update();
00152 }
00153
00154 void CasterController::setSteerCB(const std_msgs::Float64ConstPtr& msg)
00155 {
00156 steer_velocity_ = msg->data;
00157 }
00158
00159 void CasterController::setDriveCB(const std_msgs::Float64ConstPtr& msg)
00160 {
00161 drive_velocity_ = msg->data;
00162 }
00163
00164
00165
00166 }