diff --git a/docker/Dockerfile b/docker/Dockerfile
index 6011de8..cfb82f5 100644
--- a/docker/Dockerfile
+++ b/docker/Dockerfile
@@ -19,6 +19,7 @@ RUN apt-get -qq install ros-humble-desktop python3-dev python3-opencv python3-wx
# Install MavProxy
RUN pip install MAVProxy pexpect
+RUN pip3 install numpy-stl scikit-image
# Purge ModemManager if it snuck in with the above packages
RUN apt-get -qq purge modemmanager
@@ -56,8 +57,10 @@ RUN cd $WORKSPACE/ardupilot_gazebo && \
mkdir build && cd build && cmake .. && make -j4 && make install
RUN mkdir $WORKSPACE/gazebo
+RUN mkdir $WORKSPACE/misc
COPY ./gazebo $WORKSPACE/gazebo
+COPY ./misc $WORKSPACE/misc
RUN source /opt/ros/humble/setup.bash && \
cd $WORKSPACE/gazebo && \
diff --git a/docker/entrypoint.py b/docker/entrypoint.py
index fe1c8f0..35178d2 100755
--- a/docker/entrypoint.py
+++ b/docker/entrypoint.py
@@ -20,8 +20,8 @@ def template(templateFileName, values):
def run_simulation(args):
processes = []
# Start Gazebo
- processes.append(subprocess.Popen("/entrypoint.sh ros2 launch gazebo_ros gazebo.launch.py gui:={} world:=worlds/empty_sky.world".format("{}".format(args.gui).lower()), shell=True))
-
+ processes.append(subprocess.Popen("/entrypoint.sh gazebo -s libgazebo_ros_init.so -s libgazebo_ros_factory.so worlds/empty_sky.world".format("{}".format(args.gui).lower()), shell=True))
+
time.sleep(3)
tempfiles = []
diff --git a/gazebo/src/dragonfly_sim/models/co2_shell/cube.stl b/gazebo/src/dragonfly_sim/models/co2_shell/cube.stl
new file mode 100644
index 0000000..f99b3f3
Binary files /dev/null and b/gazebo/src/dragonfly_sim/models/co2_shell/cube.stl differ
diff --git a/gazebo/src/dragonfly_sim/models/co2_shell/model.config b/gazebo/src/dragonfly_sim/models/co2_shell/model.config
new file mode 100644
index 0000000..8b4814b
--- /dev/null
+++ b/gazebo/src/dragonfly_sim/models/co2_shell/model.config
@@ -0,0 +1,11 @@
+
+
+ co2_shell
+ 1.0
+ model.sdf
+
+
+
+
+
+
diff --git a/gazebo/src/dragonfly_sim/models/co2_shell/model.sdf b/gazebo/src/dragonfly_sim/models/co2_shell/model.sdf
new file mode 100644
index 0000000..1a1bab1
--- /dev/null
+++ b/gazebo/src/dragonfly_sim/models/co2_shell/model.sdf
@@ -0,0 +1,40 @@
+
+
+
+
+
+ 0
+ 0 0 0 0 -0 0
+
+ 0
+ 0
+ 0
+ 0 0 0 0 -0 0
+ 0
+
+
+
+ model://co2_shell/cube.stl
+ 1 1 1
+
+
+
+
+
+ __default__
+
+ 0 0 0 1
+ 0 0 0 1
+ 0 0 0 1
+ 0 0 0 1
+ 1
+
+ 0 0 0 0 -0 0
+ 0.5
+ 0
+
+
+ 1
+ 1
+
+
diff --git a/gazebo/src/dragonfly_sim/models/co2_sphere/model.config b/gazebo/src/dragonfly_sim/models/co2_sphere/model.config
new file mode 100644
index 0000000..8650933
--- /dev/null
+++ b/gazebo/src/dragonfly_sim/models/co2_sphere/model.config
@@ -0,0 +1,11 @@
+
+
+ co2_sphere
+ 1.0
+ model.sdf
+
+
+
+
+
+
diff --git a/gazebo/src/dragonfly_sim/models/co2_sphere/model.sdf b/gazebo/src/dragonfly_sim/models/co2_sphere/model.sdf
new file mode 100644
index 0000000..cf8175c
--- /dev/null
+++ b/gazebo/src/dragonfly_sim/models/co2_sphere/model.sdf
@@ -0,0 +1,49 @@
+
+
+
+
+
+ 1
+
+ 0.1
+ 0
+ 0
+ 0.1
+ 0
+ 0.1
+
+ 0 0 0 0 -0 0
+
+ 0
+ 0
+ 0
+ 0 0 0 0 -0 0
+ 0
+
+
+
+ 0.5
+
+
+
+
+
+ __default__
+
+ 0.3 0.3 0.3 1
+ 0.7 0.7 0.7 1
+ 0.01 0.01 0.01 1
+ 0 0 0 1
+
+ 0 0 0 0 -0 0
+ 0.5
+ 1
+
+
+ 1
+ 1
+
+
diff --git a/misc/co2_plume_spawner.py b/misc/co2_plume_spawner.py
new file mode 100755
index 0000000..f325ab2
--- /dev/null
+++ b/misc/co2_plume_spawner.py
@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+from stl import mesh
+import numpy as np
+import math
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+import random
+from skimage import measure
+from skimage.draw import ellipsoid
+
+ros1 = True
+try:
+ import rospy # ros1
+except ModuleNotFoundError:
+ ros1 = False
+ import rclpy # ros2
+ from gazebo_msgs.srv import SpawnEntity
+
+from geometry_msgs.msg import Point, Pose
+from gazebo_msgs.srv import SpawnModel
+
+"""
+To try it out try something like this
+docker exec -it `docker ps | grep dragonfly-sim | cut -d" " -f1` /bin/bash
+source /opt/ros/galactic/setup.bash
+cd workspace
+python3.8 ./misc/co2_plume_spawner.py
+"""
+
+THRESHOLD = 300 # ppm
+node = None
+spawn_model = None
+spawn_entity_client = None
+
+
+def plume(x, y, z, q=5000, k=2, u=1):
+ return (q / (2 * math.pi * k * x)) * np.exp(- (u * (pow(y, 2) + pow(z, 2)) / (4 * k * x)))
+
+
+def plume_grid():
+ x, y, z = np.mgrid[000.1:100:1, -20:20:1, 0:400:1]
+ return plume(x, y, z)
+
+
+plume_base = plume_grid()
+
+xml = open("./gazebo/src/dragonfly_sim/models/co2_shell/model.sdf", "r").read().replace("\n", "")
+
+if ros1:
+ spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
+ rospy.init_node("co2_plume_spawner")
+else:
+ rclpy.init()
+ node = rclpy.create_node('co2_plume_spawner')
+ spawn_entity_client = node.create_client(srv_type=SpawnEntity, srv_name='spawn_entity')
+ spawn_entity_client.wait_for_service(timeout_sec=1.0)
+
+for threshold_div in range(1, 6):
+ # Use marching cubes to obtain the surface mesh of these ellipsoids
+ vertices, faces, _, values = measure.marching_cubes(plume_base, THRESHOLD / threshold_div)
+ cube = mesh.Mesh(np.zeros(2 * faces.shape[0], dtype=mesh.Mesh.dtype))
+ for i, f in enumerate(faces):
+ for j in range(3):
+ cube.vectors[i][j] = vertices[f[j], :]
+ for i, f in enumerate(faces):
+ for j in range(3):
+ cube.vectors[i + len(faces)][j] = vertices[f[2-j], :]
+
+ stl_filename = '/tmp/plume' + str(threshold_div) + '.stl'
+ cube.save(stl_filename)
+ plume_co2_avg = sum(values) / len(values)
+
+ new_xml = xml.replace("model://co2_shell/cube.stl", "{}".format(stl_filename)).replace(
+ "0 0 0 1", "{red} {green} {blue} {trans}".format(
+ red=(6 - threshold_div) / 6.0,
+ green=0,
+ blue=threshold_div / 6.0,
+ trans=0.5))
+ if ros1:
+ spawn_model("plume" + str(threshold_div), new_xml, "", Pose(), "world")
+ else: # ros2
+ req = SpawnEntity.Request(xml=new_xml)
+ req.name = "plume" + str(threshold_div)
+ req.initial_pose.position = Point(x=0.0, y=0.0, z=0.0) # shell_offset
+ spawn_entity_client.call_async(req)
+if not ros1:
+ rclpy.spin_once(node)
+print("Done")