drc_plugin.h
Go to the documentation of this file.
00001 #include <boost/bind.hpp>
00002 #include <gazebo/gazebo.hh>
00003 #include <gazebo/physics/physics.hh>
00004 #include <gazebo/common/common.hh>
00005 #include <gazebo/sensors/sensors.hh>
00006 #include <gazebo/math/Vector3.hh>
00007 #include <stdio.h>
00008 
00010 
00011 #include "GazeboLANData.h"
00012 #include <sys/mman.h>
00013 #include <fcntl.h>
00014 #include <signal.h>
00015 #include <errno.h>
00016 #include <gazebo/common/Plugin.hh>
00017 #include <gazebo/physics/JointWrench.hh>
00018 
00019 
00020 // for socket client
00021 #include <stdio.h>
00022 #include <string.h>
00023 #include <sys/socket.h>
00024 #include <arpa/inet.h>
00025 
00026 
00027 #include <gazebo/common/Plugin.hh>
00028 #include <ros/ros.h>
00029 #include <tf/transform_listener.h>
00030 
00031 namespace gazebo{
00032 
00033 
00034 typedef struct _GO_DATA_{
00035     float   delta;
00036     float   current;
00037     float   togo;
00038     float   init;
00039     long    goalCnt;
00040     long    curCnt;
00041     int     flag;
00042 }GO_DATA;
00043 
00044 class DRCPlugin : public ModelPlugin
00045 {
00046 public:
00047     void    Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00048     void    OnUpdate(const common::UpdateInfo &);
00049     void    OnLAFTUpdate();
00050     void    OnRAFTUpdate();
00051     void    OnLWFTUpdate();
00052     void    OnRWFTUpdate();
00053     void    OnIMUUpdate();
00054 
00055 private:
00056     // Pointer to the model
00057     physics::ModelPtr   model;
00058     sdf::ElementPtr     element;
00059 
00060     // Pointer to the update event connection
00061     event::ConnectionPtr UpdateConnection;
00062     event::ConnectionPtr LAFTupdateConnection;
00063     event::ConnectionPtr RAFTupdateConnection;
00064     event::ConnectionPtr IMUupdateConnection;
00065 
00066 
00067     //Joint controller
00068     physics::JointController    *JCon;
00069     physics::JointPtr           JPtrs[NO_OF_JOINTS];
00070     physics::JointPtr           JPtr_LHAND[9];
00071     physics::JointPtr           JPtr_RHAND[9];
00072     physics::JointPtr           JPtr_RAFT, JPtr_LAFT;
00073     physics::JointPtr           JPtr_RWFT, JPtr_LWFT;
00074 
00075     physics::JointPtr           JPtr_RFWH, JPtr_LFWH;
00076     physics::JointPtr           JPtr_RFUNI, JPtr_LFUNI;
00077     physics::JointPtr           JPtr_Head;
00078     float                       PIDGains[NO_OF_JOINTS][3];
00079     GO_DATA                     GainOverride[NO_OF_JOINTS];
00080 
00081     void    setGainOverride(int joint, float gain, long msTime);
00082     void    moveGainOverride(int joint);
00083     void    adjustAllGain();
00084 
00085     void    findHome(int jNum);
00086 
00087     //common::Time UpdateTime, UpdatePeriod;
00088 
00089     gazebo::sensors::ForceTorqueSensorPtr   LAFT, RAFT;
00090     sensors::ImuSensorPtr                   IMU;
00091 
00092     math::Vector3   filt_LAF;
00093     math::Vector3   filt_LAT;
00094     math::Vector3   filt_RAF;
00095     math::Vector3   filt_RAT;
00096     math::Vector3   filt_LWF;
00097     math::Vector3   filt_LWT;
00098     math::Vector3   filt_RWF;
00099     math::Vector3   filt_RWT;
00100     double          filt_alpha;
00101 
00102     // reference ====
00103     double  refs[NO_OF_JOINTS];
00104     double  ref_RHAND, ref_LHAND;
00105     double  ref_RWH, ref_LWH;
00106 
00107     double  home_ref_RWH, home_ref_LWH;
00108 
00109     double  prev_refs[NO_OF_JOINTS];
00110     double  inc_refs[NO_OF_JOINTS];
00111     int     new_ref_cnt;
00112 
00113 
00114 
00115     tf::TransformListener listener;
00116     tf::StampedTransform left;
00117     tf::StampedTransform right;
00118 
00119     // socket ==========
00120     int sock;
00121     struct sockaddr_in  client;
00122     pthread_t   LANTHREAD_t;
00123 
00124     int     threadWorking;
00125     int     connectionStatus;
00126 
00127     DRC_GAZEBO_JOINT    RXJointData;
00128     DRC_GAZEBO_SENSOR   TXSensorData;
00129     int     RXDataSize;
00130     int     TXDataSize;
00131     void*   RXBuffer;
00132     void*   TXBuffer;
00133 
00134     DRC_GAZEBO_GO_CMD   GainOverrideCMD;
00135     int     HomeJoint;
00136 
00137     int     FirstRcvData;
00138     int     FirstRcvCnt;
00139     int     SimulateOk;
00140 
00141     int     CreateSocket(const char *addr, int port);
00142     int     Connect2Server();
00143     void    NewRXData();
00144     void    FirstData();
00145     static void    *LANThread(void *_arg);
00146 
00147 };
00148 
00149 
00150 
00151 }// namespace gazebo


drc_plugin
Author(s): JeongsooLim , SeungwooHong
autogenerated on Sat Jun 8 2019 20:42:28