From 4f11a59b282989f52b5f6100d97b566a85ab4d36 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Wed, 9 Mar 2022 12:30:06 -0700 Subject: [PATCH 01/10] docker/entrypoint.py: add ros factory library enabling spawning Also changed to just launching gazebo instead of a ros2 launch as the library arguments don't seem to be available in the ros2 launch. Note if there are multiple versions of ros/gazebo in the container this may cause issues --- docker/entrypoint.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 = [] From 325adcaa54a46a54b6d2c28b87641523ef8c0407 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Wed, 9 Mar 2022 12:43:22 -0700 Subject: [PATCH 02/10] gazebo/src/dragonfly_sim/models/co2_sphere/model.sdf|config: creation --- .../models/co2_sphere/model.config | 11 +++++ .../dragonfly_sim/models/co2_sphere/model.sdf | 49 +++++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 gazebo/src/dragonfly_sim/models/co2_sphere/model.config create mode 100644 gazebo/src/dragonfly_sim/models/co2_sphere/model.sdf 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 + + From 7cebb6f0ae5b273f77f14d28f52fda5c2529de00 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Wed, 27 Apr 2022 16:20:42 -0600 Subject: [PATCH 03/10] misc/co2_plume_spawner.py: plume stl generation and model spawn --- misc/co2_plume_spawner.py | 56 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 misc/co2_plume_spawner.py diff --git a/misc/co2_plume_spawner.py b/misc/co2_plume_spawner.py new file mode 100644 index 0000000..79dbd91 --- /dev/null +++ b/misc/co2_plume_spawner.py @@ -0,0 +1,56 @@ +from stl import mesh +from geometry_msgs.msg import Point +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 +import rclpy +from geometry_msgs.msg import Point +from gazebo_msgs.srv import SpawnEntity + +THRESHOLD = 30 # ppm + + +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("/home/carter/dragonfly-sim/gazebo/src/dragonfly_sim/models/co2_shell/model.sdf", "r").read().replace("\n", + "") +rclpy.init() +node = rclpy.create_node('co2_plume_spawner2') +spawn_entity_client = node.create_client(srv_type=SpawnEntity, srv_name='spawn_entity') +spawn_entity_client.wait_for_service(timeout_sec=1.0) + +rclpy.spin_once(node) +for thres_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 / thres_div) + cube = mesh.Mesh(np.zeros(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], :] + stl_filename = '/tmp/plume' + str(thres_div) + '.stl' + cube.save(stl_filename) + plume_co2_avg = sum(values) / len(values) + req = SpawnEntity.Request() + req.xml = xml.replace("model://co2_shell/cube.stl", "model://{}".format(stl_filename)) + req.xml = req.xml.replace("0 1 0 0", "{red} {green} {blue} 0".format( + red=255 / plume_co2_avg, + green=plume_co2_avg, blue=0)) + req.initial_pose.position = Point(x=0.0, y=0.0, z=0.0) # shell_offset + spawn_entity_client.call_async(req) +print("Done") +rclpy.spin(node) + + From 4779b20a8f262b80a0142a24aa2c9bd83a05bcc9 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 8 Apr 2022 17:18:49 -0600 Subject: [PATCH 04/10] gazebo/src/dragonfly_sim/models/co2_shell: init model creation using stl --- .../dragonfly_sim/models/co2_shell/cube.stl | Bin 0 -> 684 bytes .../models/co2_shell/model.config | 11 +++++ .../dragonfly_sim/models/co2_shell/model.sdf | 40 ++++++++++++++++++ 3 files changed, 51 insertions(+) create mode 100644 gazebo/src/dragonfly_sim/models/co2_shell/cube.stl create mode 100644 gazebo/src/dragonfly_sim/models/co2_shell/model.config create mode 100644 gazebo/src/dragonfly_sim/models/co2_shell/model.sdf 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 0000000000000000000000000000000000000000..f99b3f3e46d813d1d9ea3287227ccce89a058691 GIT binary patch literal 684 zcmaiuO%8%U3`RMChnS@{4;jghubc8-hLN|G}^vE=iF&Jtx z*yuS+O%?4%%x-j%91ODl7;1OC@!8*y%{#G|X-(ZW74dtLFOo2qM$GI|C!jv;=AtyI_F+qYx&z5`>@|OWxj5p?siLU?v literal 0 HcmV?d00001 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..24efc17 --- /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 1 0 0 + 0 0 0 1 + 0 0 0 1 + 1 + + 0 0 0 0 -0 0 + 0.5 + 0 + + + 1 + 1 + + From 830b25c3ae50f6d16db14bc2b6a424fc42674e5f Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Thu, 28 Apr 2022 14:27:54 -0600 Subject: [PATCH 05/10] gazebo/src/dragonfly_sim/models/co2_shell/model.sdf:remove diffuse light --- gazebo/src/dragonfly_sim/models/co2_shell/model.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/src/dragonfly_sim/models/co2_shell/model.sdf b/gazebo/src/dragonfly_sim/models/co2_shell/model.sdf index 24efc17..1a1bab1 100644 --- a/gazebo/src/dragonfly_sim/models/co2_shell/model.sdf +++ b/gazebo/src/dragonfly_sim/models/co2_shell/model.sdf @@ -24,7 +24,7 @@ __default__ 0 0 0 1 - 0 1 0 0 + 0 0 0 1 0 0 0 1 0 0 0 1 1 From 7c9ca5acfc27db6cd6e18d7cca7b4c7ff737b59f Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Thu, 28 Apr 2022 14:28:57 -0600 Subject: [PATCH 06/10] misc/co2_plume_spawner.py: spawn working on ros noetic --- misc/co2_plume_spawner.py | 66 ++++++++++++++++++++++----------------- 1 file changed, 38 insertions(+), 28 deletions(-) diff --git a/misc/co2_plume_spawner.py b/misc/co2_plume_spawner.py index 79dbd91..0e1fc73 100644 --- a/misc/co2_plume_spawner.py +++ b/misc/co2_plume_spawner.py @@ -1,5 +1,4 @@ from stl import mesh -from geometry_msgs.msg import Point import numpy as np import math import matplotlib.pyplot as plt @@ -7,50 +6,61 @@ import random from skimage import measure from skimage.draw import ellipsoid -import rclpy -from geometry_msgs.msg import Point -from gazebo_msgs.srv import SpawnEntity +# import rclpy #ros2 +import rospy # ros1 +from geometry_msgs.msg import Point, Pose +# from gazebo_msgs.srv import SpawnEntity +from gazebo_msgs.srv import SpawnModel -THRESHOLD = 30 # ppm +THRESHOLD = 300 # ppm 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))) + 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) + 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("/home/carter/dragonfly-sim/gazebo/src/dragonfly_sim/models/co2_shell/model.sdf", "r").read().replace("\n", - "") -rclpy.init() -node = rclpy.create_node('co2_plume_spawner2') -spawn_entity_client = node.create_client(srv_type=SpawnEntity, srv_name='spawn_entity') -spawn_entity_client.wait_for_service(timeout_sec=1.0) +xml = open("./gazebo/src/dragonfly_sim/models/co2_shell/model.sdf", "r").read().replace("\n", "") +# rclpy.init() +# node = rclpy.create_node('co2_plume_spawner2') +# spawn_entity_client = node.create_client(srv_type=SpawnEntity, srv_name='spawn_entity') +# spawn_entity_client.wait_for_service(timeout_sec=1.0) +# rclpy.spin_once(node) + +spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) +rospy.init_node("co2_plume_spawner2") -rclpy.spin_once(node) for thres_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 / thres_div) - cube = mesh.Mesh(np.zeros(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], :] - stl_filename = '/tmp/plume' + str(thres_div) + '.stl' - cube.save(stl_filename) - plume_co2_avg = sum(values) / len(values) + # Use marching cubes to obtain the surface mesh of these ellipsoids + vertices, faces, _, values = measure.marching_cubes(plume_base, THRESHOLD / thres_div) + cube = mesh.Mesh(np.zeros(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], :] + stl_filename = '/tmp/plume' + str(thres_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 - thres_div) / 6.0, + green=0, + blue=thres_div / 6.0, + trans=0.5)) + spawn_model("plume" + str(thres_div), new_xml, "", Pose(), "world") + ''' req = SpawnEntity.Request() req.xml = xml.replace("model://co2_shell/cube.stl", "model://{}".format(stl_filename)) req.xml = req.xml.replace("0 1 0 0", "{red} {green} {blue} 0".format( red=255 / plume_co2_avg, green=plume_co2_avg, blue=0)) req.initial_pose.position = Point(x=0.0, y=0.0, z=0.0) # shell_offset - spawn_entity_client.call_async(req) + #spawn_entity_client.call_async(req) + ''' print("Done") -rclpy.spin(node) - - From 621903549b3de053a62effd5309b8293593355e1 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 6 May 2022 13:33:19 -0600 Subject: [PATCH 07/10] docker/Dockerfile: +copy misc, pip install numpy-stl & scikit-image --- docker/Dockerfile | 3 +++ 1 file changed, 3 insertions(+) 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 && \ From 7d4657e79f3cb6fb4335a9ee98c6726d4353606a Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 6 May 2022 13:34:34 -0600 Subject: [PATCH 08/10] misc/co2_plume_spawner.py: completed ros1 & 2 compatible --- misc/co2_plume_spawner.py | 66 +++++++++++++++++++++++++-------------- 1 file changed, 42 insertions(+), 24 deletions(-) mode change 100644 => 100755 misc/co2_plume_spawner.py diff --git a/misc/co2_plume_spawner.py b/misc/co2_plume_spawner.py old mode 100644 new mode 100755 index 0e1fc73..4f89fd5 --- a/misc/co2_plume_spawner.py +++ b/misc/co2_plume_spawner.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 from stl import mesh import numpy as np import math @@ -6,13 +7,30 @@ import random from skimage import measure from skimage.draw import ellipsoid -# import rclpy #ros2 -import rospy # ros1 + +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 SpawnEntity 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): @@ -27,40 +45,40 @@ def plume_grid(): plume_base = plume_grid() xml = open("./gazebo/src/dragonfly_sim/models/co2_shell/model.sdf", "r").read().replace("\n", "") -# rclpy.init() -# node = rclpy.create_node('co2_plume_spawner2') -# spawn_entity_client = node.create_client(srv_type=SpawnEntity, srv_name='spawn_entity') -# spawn_entity_client.wait_for_service(timeout_sec=1.0) -# rclpy.spin_once(node) -spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) -rospy.init_node("co2_plume_spawner2") +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 thres_div in range(1, 6): +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 / thres_div) + vertices, faces, _, values = measure.marching_cubes(plume_base, THRESHOLD / threshold_div) cube = mesh.Mesh(np.zeros(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], :] - stl_filename = '/tmp/plume' + str(thres_div) + '.stl' + 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 - thres_div) / 6.0, + red=(6 - threshold_div) / 6.0, green=0, - blue=thres_div / 6.0, + blue=threshold_div / 6.0, trans=0.5)) - spawn_model("plume" + str(thres_div), new_xml, "", Pose(), "world") - ''' - req = SpawnEntity.Request() - req.xml = xml.replace("model://co2_shell/cube.stl", "model://{}".format(stl_filename)) - req.xml = req.xml.replace("0 1 0 0", "{red} {green} {blue} 0".format( - red=255 / plume_co2_avg, - green=plume_co2_avg, blue=0)) + 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) - ''' + spawn_entity_client.call_async(req) +if not ros1: + rclpy.spin_once(node) print("Done") From 7d270b738d6370e59915ce4c210e47075a349be1 Mon Sep 17 00:00:00 2001 From: John Ericksen Date: Sun, 22 May 2022 15:41:02 -0600 Subject: [PATCH 09/10] Specify both sides of plume mesh faces --- misc/co2_plume_spawner.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/misc/co2_plume_spawner.py b/misc/co2_plume_spawner.py index 4f89fd5..48709e7 100755 --- a/misc/co2_plume_spawner.py +++ b/misc/co2_plume_spawner.py @@ -58,10 +58,14 @@ def plume_grid(): 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(faces.shape[0], dtype=mesh.Mesh.dtype)) + 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) From fee5a096ea67f8bb6e6dc59067d05bc903ab64cf Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Sat, 27 May 2023 01:18:08 -0600 Subject: [PATCH 10/10] misc/co2_plume_spawner.py: 2->4 spacing --- misc/co2_plume_spawner.py | 78 +++++++++++++++++++-------------------- 1 file changed, 39 insertions(+), 39 deletions(-) diff --git a/misc/co2_plume_spawner.py b/misc/co2_plume_spawner.py index 48709e7..f325ab2 100755 --- a/misc/co2_plume_spawner.py +++ b/misc/co2_plume_spawner.py @@ -10,11 +10,11 @@ ros1 = True try: - import rospy # ros1 + import rospy # ros1 except ModuleNotFoundError: - ros1 = False - import rclpy # ros2 - from gazebo_msgs.srv import SpawnEntity + ros1 = False + import rclpy # ros2 + from gazebo_msgs.srv import SpawnEntity from geometry_msgs.msg import Point, Pose from gazebo_msgs.srv import SpawnModel @@ -34,12 +34,12 @@ 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))) + 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) + x, y, z = np.mgrid[000.1:100:1, -20:20:1, 0:400:1] + return plume(x, y, z) plume_base = plume_grid() @@ -47,42 +47,42 @@ def 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") + 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) + 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], :] + # 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) + 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) + 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) + rclpy.spin_once(node) print("Done")