$search
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_DECLARE_CLASS(pr2_mechanism_controllers, CasterController, 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 }