25 #include <octomap_msgs/conversions.h>
26 #include <geometric_shapes/shape_messages.h>
27 #include <geometric_shapes/shapes.h>
28 #include <geometric_shapes/shape_operations.h>
29 #include <rclcpp/logging.hpp>
Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose &pose)
void getActiveLinkNamesRecursive(std::vector< std::string > &active_links, const urdf::LinkConstSharedPtr &urdf_link, bool active)
Recursively traverses robot from root to get all active links.
shapes::ShapePtr constructShape(const urdf::Geometry *geom)