head_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 #include <gazebo/physics/Model.hh>
00022 
00023 
00024 // for socket client
00025 #include <stdio.h>
00026 #include <string.h>
00027 #include <sys/socket.h>
00028 #include <arpa/inet.h>
00029 
00030 
00031 #include <gazebo/common/Plugin.hh>
00032 #include <ros/ros.h>
00033 #include <sensor_msgs/JointState.h>
00034 #include <tf/transform_broadcaster.h>
00035 
00036 #include <drc_plugin/DRC_HEAD_CMD.h>
00037 #include <sensor_msgs/LaserScan.h>
00038 #include <sensor_msgs/PointCloud2.h>
00039 
00040 namespace gazebo{
00041 
00042 typedef struct _JOINT_DATA_{
00043     float   delta;
00044     float   current;
00045     float   togo;
00046     float   init;
00047     long    goalCnt;
00048     long    curCnt;
00049     int     flag;
00050     int     mode;
00051 }JOINT_DATA;
00052 
00053 typedef struct _SCAN_DATA_{
00054     float   target;
00055     long    timeMs;
00056 }SCAN_DATA;
00057 
00058 enum SCAN_MODE{
00059     SCAN_MODE_NONE = 0,
00060     SCAN_MODE_JUST_MOVE,
00061     SCAN_MODE_GOTO_START_POS,
00062     SCAN_MODE_SCAN
00063 };
00064 
00065 class HeadPlugin : public ModelPlugin
00066 {
00067 public:
00068     void    Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00069     void    OnUpdate(const common::UpdateInfo &);
00070 
00071 
00072 
00073 private:
00074     ros::Publisher  joint_pub;
00075     ros::Publisher  cloud_pub;
00076     ros::Subscriber cmd_sub;
00077     ros::Subscriber scan_sub;
00078 
00079     sensor_msgs::JointState joint_state;
00080     tf::TransformBroadcaster broadcaster;
00081     tf::Transform    toHokuyo;
00082     tf::Transform    toCamera;
00083     tf::Transform    toStreaming;
00084 
00085     sensor_msgs::PointCloud2 cloud;
00086     int     cloud_accum_cnt;
00087 
00088     void    head_cmd_callback(const drc_plugin::DRC_HEAD_CMD::ConstPtr &cmd);
00089     void    scan_callback(const sensor_msgs::LaserScanPtr &scan);
00090 
00091     // Pointer to the model
00092     physics::ModelPtr   model;
00093     sdf::ElementPtr     element;
00094 
00095     // Pointer to the update event connection
00096     event::ConnectionPtr UpdateConnection;
00097 
00098 
00099     //Joint controller
00100     physics::JointController    *JCon;
00101     physics::JointPtr           JPtr_Hokuyo;
00102 
00103     // reference ====
00104     double  ref_Hokuyo;
00105 
00106     JOINT_DATA  jointData;
00107     SCAN_DATA   scanData;
00108 
00109     void    setMoveJoint(float target, long msTime, int mode);
00110     void    moveJoint();
00111 };
00112 
00113 
00114 
00115 }// namespace gazebo


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