$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 /* 00035 * Author: Sachin Chitta and Matthew Piccoli 00036 */ 00037 00038 #include <ros/node_handle.h> 00039 #include <realtime_tools/realtime_publisher.h> 00040 #include <pr2_mechanism_controllers/BaseControllerState2.h> 00041 #include <robot_mechanism_controllers/joint_velocity_controller.h> 00042 #include <pr2_mechanism_controllers/base_kinematics.h> 00043 #include <geometry_msgs/Twist.h> 00044 #include <angles/angles.h> 00045 #include <boost/shared_ptr.hpp> 00046 #include <boost/scoped_ptr.hpp> 00047 #include <boost/thread/condition.hpp> 00048 #include <filters/transfer_function.h> 00049 00050 namespace controller 00051 { 00055 class Pr2BaseController2: public pr2_controller_interface::Controller 00056 { 00057 public: 00062 Pr2BaseController2(); 00063 00068 ~Pr2BaseController2(); 00069 00070 /* 00071 * \brief The starting method is called by the realtime thread just before 00072 * the first call to update. Overrides Controller.staring(). 00073 * @return Successful start 00074 */ 00075 void starting(); 00076 00077 bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 00078 00079 /* 00080 * \brief callback function for setting the desired velocity using a topic 00081 * @param cmd_vel Velocity command of the base in m/s and rad/s 00082 */ 00083 void setCommand(const geometry_msgs::Twist &cmd_vel); 00084 00089 geometry_msgs::Twist getCommand(); 00090 00095 BaseKinematics base_kinematics_; 00096 00100 pthread_mutex_t pr2_base_controller_lock_; 00101 00106 void update(); 00107 00111 void setJointCommands(); 00112 00116 void setDesiredCasterSteer(); 00117 00118 private: 00119 00120 ros::NodeHandle node_; 00121 00122 ros::NodeHandle root_handle_; 00123 00124 ros::Subscriber cmd_sub_; 00125 00126 ros::Subscriber cmd_sub_deprecated_; 00127 00131 double timeout_; 00132 00136 bool new_cmd_available_; 00137 00141 ros::Time last_time_; 00142 00146 ros::Time cmd_received_timestamp_; 00147 00152 geometry_msgs::Twist cmd_vel_t_; 00153 00157 geometry_msgs::Twist cmd_vel_; 00158 00162 geometry_msgs::Twist desired_vel_; 00163 00167 geometry_msgs::Twist max_vel_; 00168 00172 geometry_msgs::Twist max_accel_; 00173 00177 double max_translational_velocity_; 00178 00182 double max_rotational_velocity_; 00183 00187 double kp_caster_steer_; 00188 00192 double alpha_stall_; 00193 00197 std::vector<boost::shared_ptr<JointVelocityController> > wheel_controller_; 00198 00202 std::vector<boost::shared_ptr<JointVelocityController> > caster_controller_; 00203 00207 boost::scoped_ptr<realtime_tools::RealtimePublisher<pr2_mechanism_controllers::BaseControllerState2> > state_publisher_; 00208 00212 void computeJointCommands(const double &dT); 00213 00217 void updateJointControllers(); 00218 00222 void computeDesiredCasterSteer(const double &dT); 00223 00227 void computeDesiredWheelSpeeds(const double &dT); 00228 00232 void setDesiredWheelSpeeds(); 00233 00237 geometry_msgs::Twist interpolateCommand(const geometry_msgs::Twist &start, const geometry_msgs::Twist &end, const geometry_msgs::Twist &max_rate, const double &dT); 00238 00242 void commandCallback(const geometry_msgs::TwistConstPtr& msg); 00243 00247 geometry_msgs::Twist base_vel_msg_; 00248 00252 double eps_; 00253 00257 double cmd_vel_rot_eps_; 00258 00262 double state_publish_time_,state_publish_rate_; 00263 00267 ros::Time last_publish_time_; 00268 00272 double cmd_vel_trans_eps_; 00273 00274 00278 void publishState(const ros::Time ¤t_time); 00279 00280 bool publish_state_; 00281 00285 std::vector<control_toolbox::Pid> caster_position_pid_; 00286 00287 filters::MultiChannelTransferFunctionFilter<double> caster_vel_filter_; 00288 00289 std::vector<double> filtered_velocity_; 00290 00291 filters::MultiChannelTransferFunctionFilter<double> wheel_vel_filter_; 00292 00293 std::vector<double> filtered_wheel_velocity_; 00294 00298 std::vector<control_toolbox::Pid> wheel_pid_controllers_; 00299 }; 00300 00301 }