Program Listing for File roboplan_visualizer.hpp

↰ Return to documentation for file (include/roboplan_ros_visualization/roboplan_visualizer.hpp)

#pragma once

#include <memory>
#include <optional>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <pinocchio/multibody/geometry.hpp>

#include <roboplan/core/scene.hpp>
#include <std_msgs/msg/color_rgba.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

namespace roboplan_ros_visualization {

class RoboplanVisualizer {
public:
  RoboplanVisualizer(std::shared_ptr<const roboplan::Scene> scene, const std::string& urdf_xml,
                     const std::string& frame_id = "world", const std::string& ns = "/roboplan",
                     const std::string& group_name = "",
                     const std::optional<std_msgs::msg::ColorRGBA>& color = std::nullopt);

  visualization_msgs::msg::MarkerArray markers_from_configuration(const Eigen::VectorXd& q) const;

  void set_group(const std::string& group_name);

  static visualization_msgs::msg::MarkerArray clear_markers();

  void set_color(const std_msgs::msg::ColorRGBA& color);

  void clear_color();

private:
  std::vector<std::size_t> geometry_indices_for_group(const std::string& group_name) const;

  std::optional<visualization_msgs::msg::Marker>
  create_geometry_marker(int marker_id, const pinocchio::GeometryObject& geom_obj,
                         const pinocchio::SE3& placement) const;

  std::shared_ptr<const roboplan::Scene> scene_;

  pinocchio::GeometryModel visual_model_;

  std::string frame_id_;

  std::string ns_;

  std::string group_name_;

  std::vector<std::size_t> geometry_indices_;

  std::optional<std_msgs::msg::ColorRGBA> color_;
};

}  // namespace roboplan_ros_visualization