Fix for visualising attached shapes

exotica_core/src/kinematic_tree.cpp

@@ -793,6 +793,32 @@ void KinematicTree::PublishFrames(const std::string& tf_prefix)
  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)
+ {
+ 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);
+ }
+ 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);
+ }
+ }
  }
  shapes_pub_.publish(marker_array_msg_);
  }

exotica_core/src/scene.cpp

@@ -783,6 +783,7 @@ 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