caster_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Stuart Glaser
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   // Reads in the joints to control
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   // Prepares the namespaces for the velocity controllers
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;  // Angular velocity due to driving
00145   double ws = (WHEEL_OFFSET / WHEEL_RADIUS) * steer_velocity_;  // Angular velocity due to steering
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 }


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Apr 24 2014 15:44:51