InteractiveMarkerViewer.hpp

Go to the documentation of this file.

#ifndef AIKIDO_RVIZ_INTERACTIVEMARKERVIEWER_HPP_
#define AIKIDO_RVIZ_INTERACTIVEMARKERVIEWER_HPP_
 
#include <atomic>
#include <set>
#include <thread>
 
#include <dart/common/NameManager.hpp>
#include <dart/dynamics/Frame.hpp>
#include <dart/dynamics/SmartPointer.hpp>
#include <interactive_markers/interactive_marker_server.h>
#include <ros/ros.h>
 
#include "aikido/constraint/dart/TSR.hpp"
#include "aikido/planner/World.hpp"
#include "aikido/rviz/TSRMarker.hpp"
#include "aikido/rviz/pointers.hpp"
#include "aikido/trajectory/Trajectory.hpp"
 
namespace aikido {
namespace rviz {
 
AIKIDO_DECLARE_POINTERS(InteractiveMarkerViewer)
 
class InteractiveMarkerViewer
{
public:
  InteractiveMarkerViewer(
      const std::string& topicNamespace,
      const std::string& frameId,
      aikido::planner::WorldPtr env = nullptr);
  virtual ~InteractiveMarkerViewer();
 
  InteractiveMarkerViewer(const InteractiveMarkerViewer&) = delete;
  InteractiveMarkerViewer(const InteractiveMarkerViewer&&) = delete;
  InteractiveMarkerViewer& operator=(const InteractiveMarkerViewer&) = delete;
 
  interactive_markers::InteractiveMarkerServer& marker_server();
 
  SkeletonMarkerPtr addSkeletonMarker(
      const dart::dynamics::SkeletonPtr& skeleton);
 
  FrameMarkerPtr addFrameMarker(
      std::shared_ptr<const dart::dynamics::Frame> frame,
      double length = 0.25,
      double thickness = 0.02,
      double alpha = 1.0);
 
  TSRMarkerPtr addTSRMarker(
      const constraint::dart::TSR& tsr,
      int nSamples = 10,
      const std::string& basename = "");
 
  TrajectoryMarkerPtr addTrajectoryMarker(
      trajectory::ConstTrajectoryPtr trajectory,
      dart::dynamics::MetaSkeletonPtr skeleton,
      const dart::dynamics::Frame& frame,
      const Eigen::Vector4d& rgba = Eigen::Vector4d::Constant(0.75),
      double thickness = 0.01,
      std::size_t numLineSegments = 16u);
 
  void clearFrameMarkers();
 
  void clearTrajectoryMarkers();
 
  void setAutoUpdate(bool flag);
 
  void update();
 
protected:
  void autoUpdate();
 
  void updateSkeletonMarkers();
 
  void updateFrameMarkers();
 
  void updateTrajectoryMarkers();
 
  interactive_markers::InteractiveMarkerServer mMarkerServer;
 
  std::map<dart::dynamics::SkeletonPtr, SkeletonMarkerPtr> mSkeletonMarkers;
 
  std::set<FrameMarkerPtr> mFrameMarkers;
 
  std::set<TrajectoryMarkerPtr> mTrajectoryMarkers;
 
  dart::common::NameManager<trajectory::ConstTrajectoryPtr>
      mTrajectoryNameManager;
 
  std::atomic_bool mRunning;
 
  std::atomic_bool mUpdating;
 
  std::string mFrameId;
 
  aikido::planner::WorldPtr mWorld;
 
  mutable std::mutex mMutex;
 
  std::thread mThread;
};
 
} // namespace rviz
} // namespace aikido
 
#endif