TrajectoryMarker.hpp

Go to the documentation of this file.

#ifndef AIKIDO_RVIZ_TRAJECTORYMARKER_HPP_
#define AIKIDO_RVIZ_TRAJECTORYMARKER_HPP_
 
#include <dart/dynamics/Frame.hpp>
#include <interactive_markers/interactive_marker_server.h>
#include <visualization_msgs/InteractiveMarker.h>
 
#include "aikido/common/pointers.hpp"
#include "aikido/trajectory/Trajectory.hpp"
 
namespace aikido {
namespace rviz {
 
AIKIDO_DECLARE_POINTERS(TrajectoryMarker)
 
class TrajectoryMarker final
{
public:
  TrajectoryMarker(
      interactive_markers::InteractiveMarkerServer* markerServer,
      const std::string& frameId,
      const std::string& markerName,
      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);
 
  ~TrajectoryMarker();
 
  void setTrajectory(trajectory::ConstTrajectoryPtr trajectory);
 
  trajectory::ConstTrajectoryPtr getTrajectory() const;
 
  void setColor(const Eigen::Vector3d& rgb);
 
  Eigen::Vector3d getColor() const;
 
  void setAlpha(double alpha);
 
  double getAlpha() const;
 
  void setRGBA(const Eigen::Vector4d& rgb);
 
  Eigen::Vector4d getRBGA() const;
 
  void setThickness(double thickness);
 
  double getThickness() const;
 
  void setNumLineSegments(std::size_t numLineSegments);
 
  std::size_t getNumLineSegments() const;
 
  void update();
 
private:
  void updatePoints();
 
  visualization_msgs::Marker& getMarker();
 
  const visualization_msgs::Marker& getMarker() const;
 
  interactive_markers::InteractiveMarkerServer* mMarkerServer;
 
  visualization_msgs::InteractiveMarker mInteractiveMarker;
 
  std::string mFrameId;
 
  trajectory::ConstTrajectoryPtr mTrajectory;
 
  std::size_t mNumLineSegments;
  // TODO(JS): Consider switching to resolution once arc-length calculation is
  // available for Trajectoy.
 
  dart::dynamics::MetaSkeletonPtr mSkeleton;
 
  const dart::dynamics::Frame& mFrame;
 
  bool mNeedUpdate;
 
  bool mNeedPointsUpdate;
};
 
} // namespace rviz
} // namespace aikido
 
#endif // AIKIDO_RVIZ_TRAJECTORYMARKER_HPP_