pr2_base_controller.h
Go to the documentation of this file.
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/BaseControllerState.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 Pr2BaseController: public pr2_controller_interface::Controller
00056   {
00057     public:
00062       Pr2BaseController();
00063 
00068       ~Pr2BaseController();
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_kin_;
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::BaseControllerState> > state_publisher_;
00208 
00212       void computeJointCommands(const double &dT);
00213 
00217       void updateJointControllers();
00218 
00222       void computeDesiredCasterSteer(const double &dT);
00223 
00227       void computeDesiredWheelSpeeds();
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 &current_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 
00292 }


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Aug 27 2015 14:22:52