Rebase fix for collision shapes on robot

ipab-slmc · May 30, 2024 · 4e9ce29 · 4e9ce29
1 parent 116abf8
commit 4e9ce29
Show file tree

Hide file tree

Showing 2 changed files with 0 additions and 28 deletions.
diff --git a/exotica_core/src/kinematic_tree.cpp b/exotica_core/src/kinematic_tree.cpp
@@ -794,33 +794,6 @@ void KinematicTree::PublishFrames(const std::string& tf_prefix)
                 }
                 // Non-robot collision objects
                 else if (tree_[i].lock()->shape && (!tree_[i].lock()->closest_robot_link.lock() || !tree_[i].lock()->closest_robot_link.lock()->is_robot_link))
-                {
-                    if (tree_[i].lock()->shape->type != shapes::ShapeType::OCTREE)
-                    {
-                        visualization_msgs::Marker mrk;
-                        shapes::constructMarkerFromShape(tree_[i].lock()->shape.get(), mrk);
-                        mrk.action = visualization_msgs::Marker::ADD;
-                        mrk.frame_locked = true;
-                        mrk.id = i;
-                        mrk.ns = "CollisionObjects";
-                        mrk.color = GetColor(tree_[i].lock()->color);
-                        mrk.header.frame_id = tf_prefix + "/" + tree_[i].lock()->segment.getName();
-                        mrk.pose.orientation.w = 1.0;
-                        marker_array_msg_.markers.push_back(mrk);
-                    }
-                    // Octree
-                    else
-                    {
-                        // OcTree needs separate handling as it's not supported in constructMarkerFromShape
-                        // NB: This only supports a single OctoMap in the KinematicTree as we only have one publisher!
-                        octomap::OcTree my_octomap = *std::static_pointer_cast<const shapes::OcTree>(tree_[i].lock()->shape)->octree.get();
-                        octomap_msgs::Octomap octomap_msg;
-                        octomap_msgs::binaryMapToMsg(my_octomap, octomap_msg);
-                        octomap_msg.header.frame_id = tf_prefix + "/" + tree_[i].lock()->segment.getName();
-                        octomap_pub_.publish(octomap_msg);
-                    }
-                }
-                else if(tree_[i].lock()->shape && !tree_[i].lock()->is_robot_link)
                 {
                     if (tree_[i].lock()->shape->type != shapes::ShapeType::OCTREE)
                     {

diff --git a/exotica_core/src/scene.cpp b/exotica_core/src/scene.cpp
@@ -809,7 +809,6 @@ void Scene::UpdateSceneFrames()
 
             std::string collision_element_name = links[i]->getName() + "_collision_" + std::to_string(j);
             std::shared_ptr<KinematicElement> element = kinematica_.AddElement(collision_element_name, trans, links[i]->getName(), links[i]->getShapes()[j]);
-            element->is_robot_link = true;
             model_link_to_collision_element_map_[links[i]->getName()].push_back(element);
 
             // Set up mappings