Program Listing for File roboplan_ik_marker.hpp

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

#pragma once

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

#include <Eigen/Dense>

#include <geometry_msgs/msg/pose.hpp>
#include <roboplan/core/scene.hpp>
#include <roboplan_simple_ik/simple_ik.hpp>
#include <visualization_msgs/msg/interactive_marker.hpp>
#include <visualization_msgs/msg/interactive_marker_control.hpp>
#include <visualization_msgs/msg/interactive_marker_feedback.hpp>
#include <visualization_msgs/msg/marker.hpp>

namespace roboplan_ros_visualization {

using IkSolveFunction = std::function<std::optional<Eigen::VectorXd>(
    const Eigen::Matrix4d& target_pose, const Eigen::VectorXd& seed_configuration)>;

class RoboplanIKMarker {
public:
  RoboplanIKMarker(std::shared_ptr<const roboplan::Scene> scene, const std::string& base_link,
                   const std::string& tip_link, IkSolveFunction ik_solve_fn);

  visualization_msgs::msg::InteractiveMarker construct_imarker() const;

  std::optional<Eigen::VectorXd>
  process_feedback(const visualization_msgs::msg::InteractiveMarkerFeedback& feedback);

  void set_seed_configuration(const Eigen::VectorXd& q);

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

  std::string base_link_;

  std::string tip_link_;

  IkSolveFunction ik_solve_fn_;

  geometry_msgs::msg::Pose target_pose_;

  Eigen::VectorXd seed_configuration_;
};

}  // namespace roboplan_ros_visualization