added map plugin

uos · Nov 26, 2023 · deaa161 · deaa161
1 parent 7509b01
commit deaa161
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 8 deletions.
diff --git a/src/rmagine_embree_map_gzplugin.cpp b/src/rmagine_embree_map_gzplugin.cpp
@@ -199,7 +199,6 @@ std::unordered_map<rm::EmbreeGeometryPtr, VisualTransform> RmagineEmbreeMap::Emb
 
                 rm::Transform Tvl = to_rm(vis_pose);
                 rm::Transform Tvw = Tlw * Tvl;
-                std::string key = vis.name();
 
                 if(vis.has_geometry())
                 {
@@ -425,7 +424,7 @@ std::unordered_map<rm::EmbreeGeometryPtr, VisualTransform> RmagineEmbreeMap::Emb
                         geom->apply();
                         geom->commit();
 
-                        geom_to_visual[geom] = {key, Tiv, model_id};
+                        geom_to_visual[geom] = {vis_name, Tiv, model_id};
                     }
                 }
             }
@@ -480,13 +479,13 @@ std::unordered_set<rm::EmbreeGeometryPtr> RmagineEmbreeMap::EmbreeUpdateTransfor
             for(auto elem : visuals)
             {
                 msgs::Visual vis = elem.second;
-                std::string key = vis.name();
+                std::string vis_name = vis.name();
 
-                auto mesh_vis_it = m_visual_to_geoms.find(key);
+                auto mesh_vis_it = m_visual_to_geoms.find(vis_name);
                 if(mesh_vis_it == m_visual_to_geoms.end())
                 {
                     gzwarn << "WARNING mesh to update not found in embree. Skipping." << std::endl;
-                    gzwarn << "- key: " << key << std::endl;
+                    gzwarn << "- key: " << vis_name << std::endl;
                     continue;
                 }
 
@@ -514,7 +513,7 @@ std::unordered_set<rm::EmbreeGeometryPtr> RmagineEmbreeMap::EmbreeUpdateTransfor
 
                     } else {
                         gzwarn << "WARNING: visual not in mesh set. But it should." << std::endl;
-                        gzwarn << "- key: " << key << std::endl;
+                        gzwarn << "- key: " << vis_name << std::endl;
                     }
                 }
             }

diff --git a/src/rmagine_optix_map_gzplugin.cpp b/src/rmagine_optix_map_gzplugin.cpp
@@ -164,6 +164,8 @@ std::unordered_map<rm::OptixInstPtr, VisualTransform> RmagineOptixMap::OptixUpda
                 continue;
             }
 
+            std::string link_name = link->GetName();
+
             sdf::ElementPtr linkElem = link->GetSDF();
             if(linkElem->HasElement("rmagine_ignore"))
             {
@@ -181,6 +183,7 @@ std::unordered_map<rm::OptixInstPtr, VisualTransform> RmagineOptixMap::OptixUpda
                 msgs::Visual vis = elem.second;
                 msgs::Vector3d vis_scale = vis.scale();
                 msgs::Pose vis_pose = vis.pose();
+                std::string vis_name = vis.name();
 
                 rm::Vector Svl = to_rm(vis_scale);
 
@@ -192,7 +195,6 @@ std::unordered_map<rm::OptixInstPtr, VisualTransform> RmagineOptixMap::OptixUpda
 
                 rm::Transform Tvl = to_rm(vis_pose);
                 rm::Transform Tvw = Tlw * Tvl;
-                std::string key = vis.name();
 
                 if(vis.has_geometry())
                 {
@@ -382,6 +384,7 @@ std::unordered_map<rm::OptixInstPtr, VisualTransform> RmagineOptixMap::OptixUpda
                     for(auto inst : insts)
                     {
                         auto Tiv = inst->transform();
+                        inst->name = model_name + "/" + link_name + "/" + vis_name;
 
                         if(insts_ignore_model_transform.find(inst) == insts_ignore_model_transform.end())
                         {    
@@ -391,7 +394,7 @@ std::unordered_map<rm::OptixInstPtr, VisualTransform> RmagineOptixMap::OptixUpda
 
                         inst->apply();
                         inst->commit();
-                        inst_to_visual[inst] = {key, Tiv, model_id};
+                        inst_to_visual[inst] = {vis_name, Tiv, model_id};
                     }
                 }
             }