00001 00012 /************************************************************************ 00013 * Copyright (C) 2012 Eindhoven University of Technology (TU/e). * 00014 * All rights reserved. * 00015 ************************************************************************ 00016 * Redistribution and use in source and binary forms, with or without * 00017 * modification, are permitted provided that the following conditions * 00018 * are met: * 00019 * * 00020 * 1. Redistributions of source code must retain the above * 00021 * copyright notice, this list of conditions and the following * 00022 * disclaimer. * 00023 * * 00024 * 2. Redistributions in binary form must reproduce the above * 00025 * copyright notice, this list of conditions and the following * 00026 * disclaimer in the documentation and/or other materials * 00027 * provided with the distribution. * 00028 * * 00029 * THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR * 00030 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * 00031 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00032 * ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE * 00033 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * 00034 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT * 00035 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * 00036 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 00037 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 00038 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE * 00039 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 00040 * DAMAGE. * 00041 * * 00042 * The views and conclusions contained in the software and * 00043 * documentation are those of the authors and should not be * 00044 * interpreted as representing official policies, either expressed or * 00045 * implied, of TU/e. * 00046 ************************************************************************/ 00047 00048 #include <filters/transfer_function.h> 00049 #include "filters/filter_chain.h" 00050 #include <pr2_controller_interface/controller.h> 00051 #include <pr2_mechanism_model/joint.h> 00052 #include <ros/ros.h> 00053 #include <control_toolbox/pid.h> 00054 #include <geometry_msgs/Twist.h> 00055 00056 namespace controller{ 00057 00058 00059 class BaseController: public pr2_controller_interface::Controller 00060 { 00061 private: 00062 00070 void VelCallback(const geometry_msgs::Twist::ConstPtr& msg); 00071 00082 geometry_msgs::Twist interpolateCommand(const geometry_msgs::Twist &start, const geometry_msgs::Twist &end, const geometry_msgs::Twist &max_rate, const double &dT); 00083 00084 ros::Subscriber vel_sub; 00085 00087 pr2_mechanism_model::JointState* wheel_inner_left_front_state; 00088 pr2_mechanism_model::JointState* wheel_inner_left_rear_state; 00089 pr2_mechanism_model::JointState* wheel_inner_right_front_state; 00090 pr2_mechanism_model::JointState* wheel_inner_right_rear_state; 00092 00094 double current_pos_x; 00095 double current_pos_y; 00096 double current_pos_phi; 00097 00098 double desired_pos_x; 00099 double desired_pos_y; 00100 double desired_pos_phi; 00102 00104 double trans_vel_min; 00105 double rot_vel_min; 00107 00109 double Kp_x; 00110 double Kd_x; 00111 00112 double Kp_y; 00113 double Kd_y; 00114 00115 double Kp_phi; 00116 double Kd_phi; 00118 00120 double mass_x; 00121 double mass_y; 00122 double inertia_phi; 00123 00124 double damping_x; 00125 double damping_y; 00126 double damping_phi; 00127 00128 double u_x_max; 00129 double u_y_max; 00130 double u_phi_max; 00131 00132 double max_translational_velocity_; 00133 double max_rotational_velocity_; 00135 00136 double timeout_; 00137 00139 geometry_msgs::Twist cmd_vel_; 00140 00141 geometry_msgs::Twist ref_vel_last_; 00142 00143 geometry_msgs::Twist max_accel_; 00145 00146 pr2_mechanism_model::RobotState *robot_; 00147 00149 ros::Time time_of_last_cycle_; 00150 ros::Time cmd_received_timestamp_; 00151 ros::Time current_time; 00153 00154 public: 00155 00159 BaseController() ; 00160 00164 ~BaseController() ; 00165 00175 virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 00176 00181 virtual void starting(); 00182 00191 virtual void update(); 00192 00196 virtual void stopping(); 00197 }; 00198 00199 00200 }