tulip_controller.h
Go to the documentation of this file.
00001 
00013 /************************************************************************
00014  *      Copyright (C) 2012 Eindhoven University of Technology (TU/e).           *
00015  *      All rights reserved.                                                                                            *
00016  ************************************************************************
00017  *      Redistribution and use in source and binary forms, with or without      *
00018  *      modification, are permitted provided that the following conditions      *
00019  *      are met:                                                                                                                        *
00020  *                                                                                                                                              *
00021  *              1.      Redistributions of source code must retain the above            *
00022  *                      copyright notice, this list of conditions and the following *
00023  *                      disclaimer.                                                                                                     *
00024  *                                                                                                                                              *
00025  *              2.      Redistributions in binary form must reproduce the above         *
00026  *                      copyright notice, this list of conditions and the following *
00027  *                      disclaimer in the documentation and/or other materials          *
00028  *                      provided with the distribution.                                                         *
00029  *                                                                                                                                              *
00030  *      THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR            *
00031  *      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED          *
00032  *      WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE      *
00033  *      ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE        *
00034  *      FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            *
00035  *      CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT       *
00036  *      OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
00037  *      OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF           *
00038  *      LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                       *
00039  *      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE       *
00040  *      USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH        *
00041  *      DAMAGE.                                                                                                                         *
00042  *                                                                                                                                              *
00043  *      The views and conclusions contained in the software and                         *
00044  *      documentation are those of the authors and should not be                        *
00045  *      interpreted as representing official policies, either expressed or      *
00046  *      implied, of TU/e.                                                                                                       *
00047  ************************************************************************/
00048 
00049 // http://www.ros.org/wiki/pr2_mechanism
00050 // http://www.ros.org/wiki/pr2_mechanism/Tutorials
00051 
00052 #include <cstring>
00053 #include <string>
00054 #include <iostream>
00055 #include <pr2_controller_interface/controller.h>
00056 #include <pr2_mechanism_model/joint.h>
00057 #include <ros/ros.h>
00058 #include <control_toolbox/pid.h>
00059 #include <std_srvs/Empty.h>
00060 #include <gazebo_msgs/ApplyBodyWrench.h>
00061 #include <gazebo_msgs/SetModelConfiguration.h>
00062 #include <gazebo_msgs/SetModelState.h>
00063 #include <gazebo_msgs/GetModelState.h>
00064 #include <gazebo_msgs/GetPhysicsProperties.h>
00065 #include <gazebo_msgs/SetPhysicsProperties.h>
00066 #include "tulip_gazebo/joint_state_message.h"
00067 #include <ros/console.h> // for ROS_DEBUG and ROS_INFO messages
00068 #include <time.h>
00069 #define _USE_MATH_DEFINES
00070 #include <cmath>
00071 #include <tf/transform_broadcaster.h>
00072 
00073 #define JOINT_PUBLISH_TIME 5 //seconds
00074 
00075 namespace tulip_controller_namespace{
00076 
00077   
00078 enum
00079 {
00080   //Here we declare the length of the storage buffer.
00081   //StoreLen = 8005
00082   
00083   StoreLen = 16*100*JOINT_PUBLISH_TIME + 5
00084   //in the future read from parameters!
00085   //16000 samples corresponds to 16 joints * 100 times per second * 10 seconds time window, 5 extra samples to be sure
00086 };
00087 
00088 class tulip_controller_class: public pr2_controller_interface::Controller
00089 {
00090 private:
00091 
00092   bool capture();
00093   ros::Publisher     joint_state_pub_;
00094   tulip_gazebo::joint_state_message storage_[StoreLen];
00095   volatile int storage_index_;
00096   volatile int   cycle_index_;
00097   std::vector<control_toolbox::Pid> pid_controller;
00098   pr2_mechanism_model::RobotState *robot_;
00099   ros::Time time_of_first_cycle_;
00100   ros::Time time_of_previous_cycle_;
00101   std::vector<pr2_mechanism_model::JointState*> joint_state;
00102   std::vector<double> joint_pos_init;
00103   std::vector<double> joint_pos_desired;
00104   std::vector<double> joint_pos_current;
00105   std::vector<double> joint_pos_feedback;
00106   std::vector<std::string> joint_name;
00107   bool published_jointdata;
00108  
00109   //create structure to store push force and initial orientation info
00110   struct xyz {
00111     double x;
00112     double y;
00113     double z;
00114   };
00115   
00116   struct rpy_prop {
00117     double roll;
00118     double pitch;
00119     double yaw;
00120   };
00121   
00122   struct quat_prop {
00123     double x;
00124     double y;
00125     double z;
00126     double w;
00127   };
00128   
00129   struct orientation_2types {
00130     quat_prop quaternion;
00131     rpy_prop  rpy;
00132   };
00133     
00134   struct pose_properties {
00135     xyz position;
00136     orientation_2types orientation;
00137   };
00138   
00139   struct twist_properties {
00140     xyz linear;
00141     xyz angular;
00142   };
00143   
00144   struct init_model_state_properties {
00145     pose_properties pose;
00146     twist_properties twist;
00147   };
00148   
00149   struct push_properties {
00150     double time;
00151     double duration;
00152     xyz force;
00153   };  
00154   
00155 
00156   struct exp_properties {
00157     std::string name;
00158     init_model_state_properties init_model_state;
00159     push_properties push;
00160   };
00161   
00162   std::vector<exp_properties> exp;
00163   
00164 public:
00165   virtual bool init(pr2_mechanism_model::RobotState *robot,ros::NodeHandle &n);
00166   virtual void starting();
00167   virtual void update();
00168   virtual void stopping();
00169   
00170   virtual bool getXyzPar  (std::string xyz_name,            xyz& xyz_var, ros::NodeHandle &n);
00171   virtual bool getExpPar  (std::string exp_name, exp_properties& exp_var, ros::NodeHandle &n);
00172   virtual bool setExpState(                      exp_properties  exp_var, ros::NodeHandle &n);
00173   virtual bool getRobotState(gazebo_msgs::GetModelState &srv_getstate, rpy_prop &cur_orient_rpy, ros::NodeHandle &n);
00174   virtual bool applyForcetoRobot(gazebo_msgs::ApplyBodyWrench &srv_wrench, ros::NodeHandle &n);
00175 };
00176 } 


tulip_gazebo
Author(s): Tim Assman, Pieter van Zutven
autogenerated on Sun Jan 26 2014 21:00:10