00001 #include <iostream>
00002 #include <ros/ros.h>
00003 #include <vector>
00004 #include <string>
00005 #include <mapping_msgs/CollisionObject.h>
00006 #include <geometric_shapes_msgs/Shape.h>
00007
00008 class AddObjectsToMap
00009 {
00010 public:
00011
00012 AddObjectsToMap();
00013 ~AddObjectsToMap(){};
00014
00015 bool parseObjectsFile(FILE* fCfg, std::vector<std::vector<double> > &objects, std::vector<std::string> &object_ids);
00016
00017 void addObjectsFromFile(std::string filename);
00018
00019 void addBox(geometry_msgs::Pose pose, std::vector<double> &dims, std::string id);
00020
00021 void addBoxes(std::vector<std::vector<double> > &objects);
00022
00023 void addBoxes(std::vector<std::vector<double> > &objects, std::vector<std::string> &object_ids);
00024
00025 void printObjects(FILE * fOut);
00026
00027 private:
00028
00029 ros::NodeHandle nh_;
00030 ros::Publisher object_in_map_pub_;
00031
00032 std::vector<std::vector<double> > objects_;
00033 std::vector<std::string> object_ids_;
00034 };
00035
00036
