00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #ifndef GAZEBO_ROS_F3D_HH
00028 #define GAZEBO_ROS_F3D_HH
00029
00030
00031 #include <ros/callback_queue.h>
00032 #include <ros/advertise_options.h>
00033
00034 #include <gazebo/Controller.hh>
00035 #include <gazebo/Entity.hh>
00036 #include <gazebo/Model.hh>
00037 #include <gazebo/Body.hh>
00038 #include <gazebo/Param.hh>
00039
00040 #include <ros/ros.h>
00041 #include <boost/thread/mutex.hpp>
00042 #include <geometry_msgs/WrenchStamped.h>
00043
00044 namespace gazebo
00045 {
00046
00049
00071
00072
00073 class GazeboRosF3D : public Controller
00074 {
00077 public: GazeboRosF3D(Entity *parent);
00078
00080 public: virtual ~GazeboRosF3D();
00081
00084 protected: virtual void LoadChild(XMLConfigNode *node);
00085
00087 protected: virtual void InitChild();
00088
00090 protected: virtual void UpdateChild();
00091
00093 protected: virtual void FiniChild();
00094
00096 private: Model *myParent;
00097
00099 private: Body *myBody;
00100
00101
00103 private: ros::NodeHandle* rosnode_;
00104 private: ros::Publisher pub_;
00105
00107 private: geometry_msgs::WrenchStamped wrenchMsg;
00108
00110 private: ParamT<std::string> *bodyNameP;
00111 private: std::string bodyName;
00112
00114 private: ParamT<std::string> *topicNameP;
00115 private: std::string topicName;
00116
00119 private: ParamT<std::string> *frameNameP;
00120 private: std::string frameName;
00121
00123 private: ParamT<std::string> *robotNamespaceP;
00124 private: std::string robotNamespace;
00125
00127 private: boost::mutex lock;
00128
00130 private: int f3dConnectCount;
00131 private: void F3DConnect();
00132 private: void F3DDisconnect();
00133
00134
00135 private: ros::CallbackQueue queue_;
00136 private: void QueueThread();
00137 private: boost::thread callback_queue_thread_;
00138
00139 };
00140
00142
00143
00144
00145 }
00146
00147 #endif
00148