00001 #ifndef GAZEBOINTERFACE_H 00002 #define GAZEBOINTERFACE_H 00003 00004 #include "RobotController.h" 00005 #include "r2_msgs/JointControl.h" 00006 #include "r2_msgs/JointStatus.h" 00007 00008 #include "common/Plugin.hh" 00009 #include "common/Events.hh" 00010 00011 #include <ros/ros.h> 00012 #include <sensor_msgs/JointState.h> 00013 00014 namespace gazebo 00015 { 00016 class GazeboInterface : public ModelPlugin 00017 { 00018 public: 00019 GazeboInterface(); 00020 ~GazeboInterface(); 00021 00022 virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 00023 virtual void Init(); 00024 00025 // Called by the world update start event 00026 void update(); 00027 00028 private: 00029 // handle joint commands 00030 void commandJoints(const sensor_msgs::JointState::ConstPtr& msg); 00031 00032 // handle joint control messages 00033 void controlJoints(const r2_msgs::JointControl::ConstPtr& msg); 00034 00035 // traverse through yaml structure looking for the mapped values 00036 // param: structure to traverseB 00037 // valMap: returned map 00038 // searchKey: named value to find 00039 // ns: namespace to prepend 00040 // name: name of param 00041 void traverseParams(XmlRpc::XmlRpcValue param, std::map<std::string, XmlRpc::XmlRpcValue>& valMap, std::string searchKey = "", 00042 std::string ns = "", std::string name = ""); 00043 00044 // get a double from double or int XmlRpcValue 00045 // returns false if type is neither double or int 00046 bool getDoubleVal(XmlRpc::XmlRpcValue& val, double& doubleVal); 00047 00048 // Pointer to the model 00049 physics::ModelPtr modelPtr; 00050 00051 // Pointer to the update event connection 00052 event::ConnectionPtr updateConnectionPtr; 00053 00054 std::auto_ptr<RobotController> robotControllerPtr; 00055 00056 // ros stuff 00057 std::auto_ptr<ros::NodeHandle> rosNodePtr; 00058 std::auto_ptr<ros::NodeHandle> paramsNodePtr; 00059 00060 // joint commands 00061 std::string jointCommandsTopic; 00062 ros::Subscriber jointCommandsSub; 00063 00064 // joint states 00065 std::string jointStatesTopic; 00066 ros::Publisher jointStatePub; 00067 double jointStatesStepTime; 00068 00069 // joint status 00070 bool advancedMode; 00071 std::string jointControlTopic; 00072 ros::Subscriber jointControlSub; 00073 std::string jointStatusTopic; 00074 ros::Publisher jointStatusPub; 00075 double jointStatusStepTime; 00076 00077 common::Time prevStatesUpdateTime; 00078 common::Time prevStatusUpdateTime; 00079 }; 00080 } 00081 00082 #endif