tulip_controller.h

Go to the documentation of this file.

/************************************************************************
 *      Copyright (C) 2012 Eindhoven University of Technology (TU/e).           *
 *      All rights reserved.                                                                                            *
 ************************************************************************
 *      Redistribution and use in source and binary forms, with or without      *
 *      modification, are permitted provided that the following conditions      *
 *      are met:                                                                                                                        *
 *                                                                                                                                              *
 *              1.      Redistributions of source code must retain the above            *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer.                                                                                                     *
 *                                                                                                                                              *
 *              2.      Redistributions in binary form must reproduce the above         *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer in the documentation and/or other materials          *
 *                      provided with the distribution.                                                         *
 *                                                                                                                                              *
 *      THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR            *
 *      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED          *
 *      WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE      *
 *      ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE        *
 *      FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            *
 *      CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT       *
 *      OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
 *      OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF           *
 *      LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                       *
 *      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE       *
 *      USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH        *
 *      DAMAGE.                                                                                                                         *
 *                                                                                                                                              *
 *      The views and conclusions contained in the software and                         *
 *      documentation are those of the authors and should not be                        *
 *      interpreted as representing official policies, either expressed or      *
 *      implied, of TU/e.                                                                                                       *
 ************************************************************************/

// http://www.ros.org/wiki/pr2_mechanism
// http://www.ros.org/wiki/pr2_mechanism/Tutorials

#include <cstring>
#include <string>
#include <iostream>
#include <pr2_controller_interface/controller.h>
#include <pr2_mechanism_model/joint.h>
#include <ros/ros.h>
#include <control_toolbox/pid.h>
#include <std_srvs/Empty.h>
#include <gazebo_msgs/ApplyBodyWrench.h>
#include <gazebo_msgs/SetModelConfiguration.h>
#include <gazebo_msgs/SetModelState.h>
#include <gazebo_msgs/GetModelState.h>
#include <gazebo_msgs/GetPhysicsProperties.h>
#include <gazebo_msgs/SetPhysicsProperties.h>
#include "tulip_gazebo/joint_state_message.h"
#include <ros/console.h> // for ROS_DEBUG and ROS_INFO messages
#include <time.h>
#define _USE_MATH_DEFINES
#include <cmath>
#include <tf/transform_broadcaster.h>

#define JOINT_PUBLISH_TIME 5 //seconds

namespace tulip_controller_namespace{

  
enum
{
  //Here we declare the length of the storage buffer.
  //StoreLen = 8005
  
  StoreLen = 16*100*JOINT_PUBLISH_TIME + 5
  //in the future read from parameters!
  //16000 samples corresponds to 16 joints * 100 times per second * 10 seconds time window, 5 extra samples to be sure
};

class tulip_controller_class: public pr2_controller_interface::Controller
{
private:

  bool capture();
  ros::Publisher     joint_state_pub_;
  tulip_gazebo::joint_state_message storage_[StoreLen];
  volatile int storage_index_;
  volatile int   cycle_index_;
  std::vector<control_toolbox::Pid> pid_controller;
  pr2_mechanism_model::RobotState *robot_;
  ros::Time time_of_first_cycle_;
  ros::Time time_of_previous_cycle_;
  std::vector<pr2_mechanism_model::JointState*> joint_state;
  std::vector<double> joint_pos_init;
  std::vector<double> joint_pos_desired;
  std::vector<double> joint_pos_current;
  std::vector<double> joint_pos_feedback;
  std::vector<std::string> joint_name;
  bool published_jointdata;
 
  //create structure to store push force and initial orientation info
  struct xyz {
    double x;
    double y;
    double z;
  };
  
  struct rpy_prop {
    double roll;
    double pitch;
    double yaw;
  };
  
  struct quat_prop {
    double x;
    double y;
    double z;
    double w;
  };
  
  struct orientation_2types {
    quat_prop quaternion;
    rpy_prop  rpy;
  };
    
  struct pose_properties {
    xyz position;
    orientation_2types orientation;
  };
  
  struct twist_properties {
    xyz linear;
    xyz angular;
  };
  
  struct init_model_state_properties {
    pose_properties pose;
    twist_properties twist;
  };
  
  struct push_properties {
    double time;
    double duration;
    xyz force;
  };  
  

  struct exp_properties {
    std::string name;
    init_model_state_properties init_model_state;
    push_properties push;
  };
  
  std::vector<exp_properties> exp;
  
public:
  virtual bool init(pr2_mechanism_model::RobotState *robot,ros::NodeHandle &n);
  virtual void starting();
  virtual void update();
  virtual void stopping();
  
  virtual bool getXyzPar  (std::string xyz_name,            xyz& xyz_var, ros::NodeHandle &n);
  virtual bool getExpPar  (std::string exp_name, exp_properties& exp_var, ros::NodeHandle &n);
  virtual bool setExpState(                      exp_properties  exp_var, ros::NodeHandle &n);
  virtual bool getRobotState(gazebo_msgs::GetModelState &srv_getstate, rpy_prop &cur_orient_rpy, ros::NodeHandle &n);
  virtual bool applyForcetoRobot(gazebo_msgs::ApplyBodyWrench &srv_wrench, ros::NodeHandle &n);
};
}