00001 00013 #ifndef model_server_h___ 00014 #define model_server_h___ 00015 00016 #include "ros/ros.h" 00017 #include "icr.h" 00018 #include "pose_broadcaster.h" 00019 /* #include "../srv_gen/cpp/include/icr/load_model.h" */ 00020 /* #include "../srv_gen/cpp/include/icr/SetObject.h" */ 00021 #include <boost/thread/mutex.hpp> 00022 #include <boost/shared_ptr.hpp> 00023 #include <icr_msgs/LoadObject.h> 00024 #include <icr_msgs/Object.h> 00025 /* #include <tf/transform_broadcaster.h> */ 00026 #include <string> 00027 #include <vector> 00028 /* #include <iostream> */ 00029 #include <pcl/point_cloud.h> 00030 #include <pcl/point_types.h> 00031 00032 namespace ICR 00033 { 00034 00035 class ModelServer 00036 { 00037 public: 00038 00039 ModelServer(); 00040 ~ModelServer(){}; 00041 00042 void spin(); 00043 00044 private: 00045 00046 ros::NodeHandle nh_, nh_private_; 00047 boost::mutex lock_; 00048 /* tf::TransformBroadcaster tf_brc_; */ 00049 00050 std::string model_dir_; 00051 std::string obj_name_; 00052 std::string obj_frame_id_; 00053 std::string pose_source_; 00054 boost::shared_ptr<PoseBroadcaster> pose_brc_; 00055 icr_msgs::Object::Ptr obj_; 00056 double scale_; 00057 bool obj_loaded_; 00058 00059 ros::ServiceServer load_obj_srv_; 00060 ros::ServiceClient gazebo_spawn_clt_; 00061 ros::ServiceClient gazebo_delete_clt_; 00062 ros::ServiceClient gazebo_pause_clt_; 00063 ros::ServiceClient gazebo_unpause_clt_; 00064 ros::ServiceClient gazebo_get_wp_clt_; 00065 std::vector<boost::shared_ptr<ros::ServiceClient> > set_obj_clts_; 00066 00067 /* ros::Subscriber gazebo_modstat_sub_; */ 00068 00069 00070 bool loadURDF(std::string const & path,std::string & serialized_model); 00071 bool gazeboSpawnModel(std::string const & serialized_model,geometry_msgs::Pose const & initial_pose); 00072 bool gazeboDeleteModel(std::string const & name); 00073 bool loadWavefrontObj(std::string const & path, pcl::PointCloud<pcl::PointNormal> & cloud, std::vector<std::vector<unsigned int> > & neighbors ); 00074 00075 00076 00078 // CALLBACKS // 00080 00085 bool loadObject(icr_msgs::LoadObject::Request &req, icr_msgs::LoadObject::Response &res); 00086 00087 00088 }; 00089 }//end namespace 00090 00091 //-------------------------------------------------------------------------- 00092 00093 #endif