From e5abe786a3ad1f5414a58152f0d94cf78260b438 Mon Sep 17 00:00:00 2001
From: LucaKro <luc_kro@uni-bremen.de>
Date: Fri, 9 Aug 2024 13:41:12 +0200
Subject: [PATCH] [generation] temporarily fixed plugin parsing

---
 src/fpm/graph.py                         |  9 +++++++--
 src/fpm/templates/gazebo/world.sdf.jinja | 23 +++++++++++++++++++----
 src/fpm/transformations/objects.py       |  8 +++++++-
 src/fpm/utils.py                         |  3 ++-
 4 files changed, 35 insertions(+), 8 deletions(-)

diff --git a/src/fpm/graph.py b/src/fpm/graph.py
index 8c4476d..baca65d 100644
--- a/src/fpm/graph.py
+++ b/src/fpm/graph.py
@@ -1,5 +1,6 @@
 import os
 import glob
+import re
 
 import numpy as np
 
@@ -14,10 +15,16 @@
 def build_graph_from_directory(inputs: tuple):
     # Build the graph by reading all composable models in the input folder
     g = rdflib.Graph()
+
+    def natural_sort_key(s):
+        """Split the string into a list of substrings, with numbers as numbers."""
+        return [int(text) if text.isdigit() else text.lower() for text in re.split(r'(\d+)', s)]
+
     for input_folder in inputs:
         input_models = glob.glob(os.path.join(input_folder, "*.json"))
         print("Found {} models in {}".format(len(input_models), input_folder))
         print("Adding to the graph...")
+        input_models = sorted(input_models, key=natural_sort_key)
         for file_path in input_models:
             print("\t{}".format(file_path))
             g.parse(file_path, format="json-ld")
@@ -60,7 +67,6 @@ def get_floorplan_model_name(g):
 
 
 def traverse_to_world_origin(g, frame):
-
     # Go through the geometric relation predicates
     pred_filter = traversal.filter_by_predicates([GEOM["with-respect-to"], GEOM["of"]])
     # Algorithm to traverse the graph
@@ -92,7 +98,6 @@ def traverse_to_world_origin(g, frame):
 
 
 def get_transformation_matrix_wrt_frame(g, root, target):
-
     # Configure the traversal algorithm
     filter = [GEOM["with-respect-to"], GEOM["of"]]
     pred_filter = traversal.filter_by_predicates(filter)
diff --git a/src/fpm/templates/gazebo/world.sdf.jinja b/src/fpm/templates/gazebo/world.sdf.jinja
index 5cb81c2..769e83c 100644
--- a/src/fpm/templates/gazebo/world.sdf.jinja
+++ b/src/fpm/templates/gazebo/world.sdf.jinja
@@ -24,19 +24,34 @@
       <static>true</static>
       <pose>-0.0 0.0 0.0 0.0 0.0 0.0</pose>
     </include>
-    {% for instance in model.instances %}
+    {%- for instance in model.instances %}
+        {# Just here to add a newline before each include for readability#}
     <include>
       <name>{{instance.instance_name}}</name>
       <uri>model://{{instance.name}}</uri>
       <static>{{instance.static}}</static>
       <pose>{{instance.pose}}</pose>
-      {% for start_joint_state in instance.start_joint_states %}
+      {%- for start_joint_state in instance.start_joint_states %}
+          {%- if instance.plugin_type == 'initial' %}
         <plugin name="initial_plugin" filename="libinitial_plugin.so">
           <joint>{{start_joint_state.joint}}</joint>
           <position>{{start_joint_state.position}}</position>
         </plugin>
-      {% endfor %}
+          {%- elif instance.plugin_type == 'dynamic' %}
+        <plugin name="dynamic_joint_pluginss" filename="libdynamic_joint_plugin.so">
+          <joint>{{start_joint_state.joint}}</joint>
+          <uri>{{start_joint_state.uri}}</uri>
+        </plugin>
+          {%- elif instance.plugin_type == 'adversarial' %}
+        <plugin name="adversarial_joint_plugin" filename="libadversarial_joint_plugin.so">
+          <joint>{{start_joint_state.joint}}</joint>
+          <x>{{start_joint_state.position_before}}</x>
+          <near>{{start_joint_state.distance_to_trigger}}</near>
+          <y>{{start_joint_state.position_after}}</y>
+        </plugin>
+        {%- endif %}
+      {%- endfor %}
     </include>
-    {% endfor %}
+    {%- endfor %}
   </world>
 </sdf>
diff --git a/src/fpm/transformations/objects.py b/src/fpm/transformations/objects.py
index 261a799..f9cfe24 100644
--- a/src/fpm/transformations/objects.py
+++ b/src/fpm/transformations/objects.py
@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 
-from rdflib import RDF
+from rdflib import RDF, URIRef
 
 from fpm.transformations.sdf import (
     get_sdf_geometry,
@@ -149,6 +149,11 @@ def get_object_instance(g, instance):
     T = get_transformation_matrix_wrt_frame(g, frame, world_frame)
     pose_coordinates = get_sdf_pose_from_transformation_matrix(T)
 
+    plugin_type = g.value(instance, OBJ["plugin"])
+    if plugin_type:
+        plugin_uri = URIRef(plugin_type)
+        plugin_type = plugin_uri.split("#")[-1]
+
     state = g.value(instance, ST["start-state"])
     start_joint_states = []
 
@@ -173,6 +178,7 @@ def get_object_instance(g, instance):
         "static": "false",
         "name": prefixed(g, of_obj)[5:],
         "instance_name": prefixed(g, instance)[5:],
+        "plugin_type": plugin_type,
         "start_joint_states": start_joint_states,
     }
 
diff --git a/src/fpm/utils.py b/src/fpm/utils.py
index 2ae7061..6a03dff 100644
--- a/src/fpm/utils.py
+++ b/src/fpm/utils.py
@@ -19,7 +19,8 @@ def load_template(template_name, template_folder=None):
         loader = PackageLoader("fpm")
     else:
         loader = FileSystemLoader(template_folder)
-    env = Environment(loader=loader)
+    env = Environment(loader=loader,
+                      lstrip_blocks=True)
     return env.get_template(template_name)