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/sensors/DepthCameraSensor.hh>
00018 #include <gazebo/sensors/CameraSensor.hh>
00019 #include <gazebo/rendering/DepthCamera.hh>
00020 #include <gazebo/physics/JointWrench.hh>
00021 
00022 
00023 // for socket client
00024 #include <stdio.h>
00025 #include <string.h>
00026 #include <sys/socket.h>
00027 #include <arpa/inet.h>
00028 
00029 
00030 #include <gazebo/common/Plugin.hh>
00031 #include <ros/ros.h>
00032 #include <sensor_msgs/JointState.h>
00033 #include <tf/transform_broadcaster.h>
00034 
00035 namespace gazebo{
00036 
00037 
00038 typedef struct _GO_DATA_{
00039     float   delta;
00040     float   current;
00041     float   togo;
00042     float   init;
00043     long    goalCnt;
00044     long    curCnt;
00045     int     flag;
00046 }GO_DATA;
00047 
00048 class DRCPlugin : public ModelPlugin
00049 {
00050 public:
00051     void    Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00052     void    OnUpdate(const common::UpdateInfo &);
00053     void    OnLAFTUpdate();
00054     void    OnRAFTUpdate();
00055     void    OnLWFTUpdate();
00056     void    OnRWFTUpdate();
00057     void    OnIMUUpdate();
00058 
00059 private:
00060     ros::Publisher joint_pub;
00061     tf::TransformBroadcaster broadcaster;
00062     sensor_msgs::JointState joint_state;
00063 
00064     // Pointer to the model
00065     physics::ModelPtr   model;
00066     sdf::ElementPtr     element;
00067 
00068     // Pointer to the update event connection
00069     event::ConnectionPtr UpdateConnection;
00070     event::ConnectionPtr LAFTupdateConnection;
00071     event::ConnectionPtr RAFTupdateConnection;
00072 //    event::ConnectionPtr LWFTupdateConnection;
00073 //    event::ConnectionPtr RWFTupdateConnection;
00074     event::ConnectionPtr IMUupdateConnection;
00075 //    event::ConnectionPtr RGBDupdateConnection;
00076 //    event::ConnectionPtr DepthupdateConnection;
00077 
00078     //Joint controller
00079     physics::JointController    *JCon;
00080     physics::JointPtr           JPtrs[NO_OF_JOINTS];
00081     physics::JointPtr           JPtr_LHAND[9];
00082     physics::JointPtr           JPtr_RHAND[9];
00083     physics::JointPtr           JPtr_RAFT, JPtr_LAFT;
00084     physics::JointPtr           JPtr_RWFT, JPtr_LWFT;
00085 
00086 //    physics::JointPtr           JPtr_RFWH, JPtr_LFWH;
00087 //    physics::JointPtr           JPtr_RFUNI, JPtr_LFUNI;
00088     physics::JointPtr           JPtr_Head;
00089     float                       PIDGains[NO_OF_JOINTS][3];
00090     GO_DATA                     GainOverride[NO_OF_JOINTS];
00091 
00092     void    setGainOverride(int joint, float gain, long msTime);
00093     void    moveGainOverride(int joint);
00094     void    adjustAllGain();
00095 
00096     void    findHome(int jNum);
00097 
00098     //common::Time UpdateTime, UpdatePeriod;
00099 
00100     gazebo::sensors::ForceTorqueSensorPtr   LAFT, RAFT;
00101     sensors::ImuSensorPtr                   IMU;
00102 //    gazebo::sensors::ForceTorqueSensorPtr   LWFT, RWFT;
00103 //    sensors::ContactSensorPtr               LWFT, RWFT;
00104 //    msgs::Contacts LFcontact, RFcontact;
00105 //    msgs::Contacts LWcontact, RWcontact;
00106 
00107     math::Vector3   filt_LAF;
00108     math::Vector3   filt_LAT;
00109     math::Vector3   filt_RAF;
00110     math::Vector3   filt_RAT;
00111     math::Vector3   filt_LWF;
00112     math::Vector3   filt_LWT;
00113     math::Vector3   filt_RWF;
00114     math::Vector3   filt_RWT;
00115     double          filt_alpha;
00116 
00117     // reference ====
00118     double  refs[NO_OF_JOINTS];
00119     double  ref_RHAND, ref_LHAND;
00120     double  ref_RWH, ref_LWH;
00121 
00122     double  home_ref_RWH, home_ref_LWH;
00123 
00124     double  prev_refs[NO_OF_JOINTS];
00125     double  inc_refs[NO_OF_JOINTS];
00126     int     new_ref_cnt;
00127 
00128 
00129     // socket ==========
00130     int sock;
00131     struct sockaddr_in  client;
00132     pthread_t   LANTHREAD_t;
00133 
00134     int     threadWorking;
00135     int     connectionStatus;
00136 
00137     DRC_GAZEBO_JOINT    RXJointData;
00138     DRC_GAZEBO_SENSOR   TXSensorData;
00139     int     RXDataSize;
00140     int     TXDataSize;
00141     void*   RXBuffer;
00142     void*   TXBuffer;
00143 
00144     DRC_GAZEBO_GO_CMD   GainOverrideCMD;
00145     int     HomeJoint;
00146 
00147     int     CreateSocket(const char *addr, int port);
00148     int     Connect2Server();
00149     void    NewRXData();
00150     static void    *LANThread(void *_arg);
00151 
00152 };
00153 
00154 
00155 
00156 }// namespace gazebo


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