00001 #ifndef GAZEBO_GAZEBOOBJECTINFO_H 00002 #define GAZEBO_GAZEBOOBJECTINFO_H 00003 00004 #include <ros/ros.h> 00005 #include <gazebo/gazebo.hh> 00006 #include <gazebo/physics/Model.hh> 00007 00008 #include <object_msgs/Object.h> 00009 #include <object_msgs/ObjectInfo.h> 00010 #include <object_msgs/ObjectInfoRequest.h> 00011 #include <object_msgs/ObjectInfoResponse.h> 00012 00013 #include <shape_msgs/SolidPrimitive.h> 00014 #include <geometry_msgs/Pose.h> 00015 00016 namespace gazebo 00017 { 00018 00049 class GazeboObjectInfo : public WorldPlugin 00050 { 00051 public: 00052 typedef object_msgs::Object ObjectMsg; 00053 typedef object_msgs::ObjectInfo ObjectInfoMsg; 00054 00055 GazeboObjectInfo(); 00056 00057 void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf); 00058 00059 private: 00060 00061 bool requestObject(object_msgs::ObjectInfo::Request &req, object_msgs::ObjectInfo::Response &res); 00062 00063 void advertEvent(const ros::TimerEvent& e); 00064 00065 00066 //publishes a model made up of its bounding boxes 00067 void onWorldUpdate(); 00068 00069 shape_msgs::SolidPrimitive * getSolidPrimitive(physics::CollisionPtr& c); 00070 00071 //returns a model made up of its bounding boxes 00072 ObjectMsg createBoundingBoxObject(physics::ModelPtr& model, bool include_shape); 00073 00074 private: 00075 //ros::NodeHandle node; 00076 bool PUBLISH_OBJECTS; 00077 std::string WORLD_OBJECTS_TOPIC; 00078 std::string REQUEST_OBJECTS_TOPIC; 00079 std::string ROOT_FRAME_ID; 00080 00081 physics::WorldPtr world; 00082 00083 event::ConnectionPtr update_connection; 00084 ros::Publisher object_pub; 00085 00086 ros::ServiceServer request_object_srv; 00087 00088 ros::Timer publishTimer; 00089 00090 std::vector<ObjectMsg> lastGeneratedObjects; 00091 bool reGenerateObjects; 00092 }; 00093 00094 } 00095 00096 #endif // GAZEBO_GAZEBOOBJECTINFO_H