diff --git a/README.md b/README.md
index c6c3f32..27fb848 100644
--- a/README.md
+++ b/README.md
@@ -110,7 +110,6 @@ We provide several sample environments, which can be selected in [swarm_explorat
```
Other examples can be found in [map_generator/resource](uav_simulator/map_generator/resource). If you want to use your own environments, simply place the .pcd files in [map_generator/resource](uav_simulator/map_generator/resource), and follow the comments above to specify it. You may also need to change the bounding box of explored space in [exploration.launch](swarm_exploration/exploration_manager/launch/single_drone_planner.xml):
-
```xml
@@ -119,6 +118,13 @@ Other examples can be found in [map_generator/resource](uav_simulator/map_genera
```
+and the map_floor parameters in the map_publisher node in [swarm_exploration.launch](swarm_exploration/exploration_manager/launch/swarm_exploration.launch) or the exploration launch file that is going to be used to match the size of the map:
+```xml
+
+
+
+
+```
To create your own .pcd environments, you can use [this tool](https://github.com/HKUST-Aerial-Robotics/FUEL#creating-a-pcd-environment).
diff --git a/swarm_exploration/exploration_manager/launch/swarm_exploration.launch b/swarm_exploration/exploration_manager/launch/swarm_exploration.launch
index 7ec11e1..49206d1 100644
--- a/swarm_exploration/exploration_manager/launch/swarm_exploration.launch
+++ b/swarm_exploration/exploration_manager/launch/swarm_exploration.launch
@@ -8,10 +8,12 @@
-
-
-
+
+
+
+
+
+
diff --git a/uav_simulator/map_generator/src/map_publisher.cpp b/uav_simulator/map_generator/src/map_publisher.cpp
index 84ebc3f..df01ee6 100644
--- a/uav_simulator/map_generator/src/map_publisher.cpp
+++ b/uav_simulator/map_generator/src/map_publisher.cpp
@@ -7,6 +7,7 @@
using namespace std;
string file_name;
+double x_min, y_min, x_max, y_max;
int main(int argc, char** argv)
{
@@ -19,6 +20,11 @@ int main(int argc, char** argv)
// file_name = "/home/boboyu/workspaces/catkin_ws/src/uav_simulator/map_generator/resource/tmp.pcd";
// ros::Publisher cloud_pub = node.advertise("/pcl_render_node/local_map", 10, true);
+ node.param("map_pub/map_floor/x_min", x_min, -10.0);
+ node.param("map_pub/map_floor/x_max", x_max, 10.0);
+ node.param("map_pub/map_floor/y_min", y_min, -10.0);
+ node.param("map_pub/map_floor/y_max", y_max, 10.0);
+
ros::Duration(1.0).sleep();
/* load cloud from pcd */
@@ -30,25 +36,20 @@ int main(int argc, char** argv)
return -1;
}
- // // Process map
- // for (int i = 0; i < cloud.points.size(); ++i)
- // {
- // auto pt = cloud.points[i];
- // pcl::PointXYZ pr;
- // pr.x = pt.x;
- // pr.y = -pt.z;
- // pr.z = pt.y;
- // cloud.points[i] = pr;
- // }
-
- for (double x = -7; x <= 7; x += 0.1)
- for (double y = -15; y <= 15; y += 0.1)
+ /*
+ for (double x = -12; x <= 12; x += 0.1)
+ for (double y = -40; y <= 40; y += 0.1)
{
cloud.push_back(pcl::PointXYZ(x, y, 0));
}
+ */
- // cout << "Publishing map..." << endl;
-
+ for (double x = x_min; x <= x_max; x += 0.1)
+ for (double y = y_min; y <= y_max; y += 0.1)
+ {
+ cloud.push_back(pcl::PointXYZ(x, y, 0));
+ }
+
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(cloud, msg);
msg.header.frame_id = "world";
@@ -62,4 +63,4 @@ int main(int argc, char** argv)
cout << "finish publish map." << endl;
return 0;
-}
\ No newline at end of file
+}