Add TrajectoryMarker (#288)

CHANGELOG.md

@@ -23,6 +23,7 @@
 * RViz
 
   * Added WorldInteractiveMarkerViewer: [#242](https://github.com/personalrobotics/aikido/pull/242)
+  * Added TrajectoryMarker: [#288](https://github.com/personalrobotics/aikido/pull/288)
 
 * IO
 

include/aikido/rviz/InteractiveMarkerViewer.hpp

@@ -5,6 +5,7 @@
 #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>
@@ -13,6 +14,7 @@
 #include <aikido/constraint/TSR.hpp>
 #include <aikido/rviz/SmartPointers.hpp>
 #include <aikido/rviz/TSRMarker.hpp>
+#include <aikido/trajectory/Trajectory.hpp>
 
 namespace aikido {
 namespace rviz {
@@ -51,6 +53,28 @@ class InteractiveMarkerViewer
       int nSamples = 10,
       const std::string& basename = "");
 
+  /// Adds trajectory marker to this viewer.
+  ///
+  /// \param[in] trajectory C-space (or joint-space) trajectory.
+  /// \param[in] skeleton Target DART meta skeleton that the C-space trajectory
+  /// will be applied to compute the visualizing task-space trajectory.
+  /// \param[in] frame Target DART frame where the trajectory of its origin
+  /// will be visualized.
+  /// \param[in] rgba Color and alpha of the visualized trajectory. Default is
+  /// [RGBA: 0.75, 0.75, 0.75, 0.75].
+  /// \param[in] thickness Thickness of the visualized trajectory. Default is
+  /// 0.01.
+  /// \param[in] numLineSegments Number of line segments in the visualized
+  /// trajectory. Default is 16.
+  /// \return Trajectory marker added to this viewer.
+  TrajectoryMarkerPtr addTrajectoryMarker(
+      trajectory::ConstTrajectoryPtr trajectory,
+      const dart::dynamics::MetaSkeleton& 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 setAutoUpdate(bool flag);
   void update();
 
@@ -60,6 +84,12 @@ class InteractiveMarkerViewer
   interactive_markers::InteractiveMarkerServer mMarkerServer;
   std::set<SkeletonMarkerPtr> mSkeletonMarkers;
   std::set<FrameMarkerPtr> mFrameMarkers;
+  std::set<TrajectoryMarkerPtr> mTrajectoryMarkers;
+
+  /// NameManager for name uniqueness of trajectories in the same
+  /// InteractiveMarkerServer.
+  dart::common::NameManager<trajectory::ConstTrajectoryPtr>
+      mTrajectoryNameManager;
 
   std::atomic_bool mRunning;
   std::atomic_bool mUpdating;

include/aikido/rviz/SmartPointers.hpp

@@ -16,6 +16,7 @@ namespace rviz {
 AIKIDO_DEFINE_SHARED_PTR(FrameMarker)
 AIKIDO_DEFINE_SHARED_PTR(BodyNodeMarker)
 AIKIDO_DEFINE_SHARED_PTR(SkeletonMarker)
+AIKIDO_DEFINE_SHARED_PTR(TrajectoryMarker)
 AIKIDO_DEFINE_SHARED_PTR(TSRMarker)
 
 } // namespace rviz

include/aikido/rviz/TrajectoryMarker.hpp

@@ -0,0 +1,143 @@
+#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/trajectory/Trajectory.hpp"
+
+namespace aikido {
+namespace rviz {
+
+/// A wrapper class of RViz InteractiveMarker for visualizing AIKIDO trajectory
+/// in RViz.
+class TrajectoryMarker final
+{
+public:
+  /// Constructor.
+  /// \param[in] markerServer RViz marker server.
+  /// \param[in] frameId RViz frame ID.
+  /// \param[in] markerName Name of the RViz InteractiveMarker associated with
+  /// this TrajectoryMarker. The name must be unique in the same
+  /// InteractiveMarkerServer. AIKIDO InteractiveMarkerViewer uses a name
+  /// manager for the name uniqueness.
+  /// \param[in] trajectory C-space (or joint-space) trajectory.
+  /// \param[in] skeleton DART meta skeleton for visualizing the trajectory in
+  /// task space.
+  /// \param[in] frame DART frame for visualizing the trajectory in task space.
+  /// The trajectory of the origin will be visualized.
+  /// \param[in] rgba Color and alpha of the visualized trajectory. Default is
+  /// [RGBA: 0.75, 0.75, 0.75, 0.75].
+  /// \param[in] thickness Thickness of the visualized trajectory. Default is
+  /// 0.01.
+  /// \param[in] numLineSegments Number of line segments in the visualized
+  /// trajectory. Default is 16.
+  TrajectoryMarker(
+      interactive_markers::InteractiveMarkerServer* markerServer,
+      const std::string& frameId,
+      const std::string& markerName,
+      trajectory::ConstTrajectoryPtr trajectory,
+      const dart::dynamics::MetaSkeleton& skeleton,
+      const dart::dynamics::Frame& frame,
+      const Eigen::Vector4d& rgba = Eigen::Vector4d::Constant(0.75),
+      double thickness = 0.01,
+      std::size_t numLineSegments = 16u);
+
+  /// Destructor
+  ~TrajectoryMarker();
+
+  /// Sets or updates trajectory to visualize
+  ///
+  /// \param[in] trajectory C-space (or joint-space) trajectory. The statespace
+  /// of the trajectory should be MetaSkeletonStateSpace. Otherwise, throws
+  /// invalid_argument exception. Passing in nullptr will clear the current
+  /// trajectory.
+  void setTrajectory(trajectory::ConstTrajectoryPtr trajectory);
+
+  /// Returns trajectory associated with this marker
+  trajectory::ConstTrajectoryPtr getTrajectory() const;
+
+  /// Sets or updates color (RGB) of visualized trajectory.
+  void setColor(const Eigen::Vector3d& rgb);
+
+  /// Returns color (RGB) of visualized trajectory.
+  Eigen::Vector3d getColor() const;
+
+  /// Sets or updates alpha of visualized trajectory.
+  void setAlpha(double alpha);
+
+  /// Returns alpha of visualized trajectory.
+  double getAlpha() const;
+
+  /// Sets or updates RGBA of visualized trajectory.
+  void setRGBA(const Eigen::Vector4d& rgb);
+
+  /// Returns RGBA of visualized trajectory.
+  Eigen::Vector4d getRBGA() const;
+
+  /// Sets or updates thickness of visualized trajectory.
+  void setThickness(double thickness);
+
+  /// Returns thickness of visualized trajectory.
+  double getThickness() const;
+
+  /// Sets or updates number of line segments of the visualizing trajectory.
+  void setNumLineSegments(std::size_t numLineSegments);
+
+  /// Returns number of line segments of the visualizing trajectory.
+  std::size_t getNumLineSegments() const;
+
+  /// Updates this marker.
+  ///
+  /// This function should be called after the properties are changed (e.g.,
+  /// trajectory, color, thickness, number of line-segments) so that RViz
+  /// reflects the changes accordingly.
+  void update();
+
+private:
+  /// Updates trajectory points based on trajectory and number of line-segments.
+  ///
+  /// This function is called by update().
+  void updatePoints();
+
+  /// Returns marker.
+  visualization_msgs::Marker& getMarker();
+
+  /// Returns const marker.
+  const visualization_msgs::Marker& getMarker() const;
+
+  /// RViz marker server.
+  interactive_markers::InteractiveMarkerServer* mMarkerServer;
+
+  /// RViz interactive marker.
+  visualization_msgs::InteractiveMarker mInteractiveMarker;
+
+  /// Frame name of RViz interactive marker.
+  std::string mFrameId;
+
+  /// C-space (or joint-space) trajectory.
+  trajectory::ConstTrajectoryPtr mTrajectory;
+
+  /// Number of line segments of the discretized task-space trajectory.
+  std::size_t mNumLineSegments;
+  // TODO(JS): Consider switching to resolution once arc-length calculation is
+  // available for Trajectoy.
+
+  /// Target DART meta skeleton that the C-space trajectory will be applied to
+  /// compute the visualizing task-space trajectory.
+  const dart::dynamics::MetaSkeleton& mSkeleton;
+
+  /// Target DART frame where the trajectory of its origin will be visualized.
+  const dart::dynamics::Frame& mFrame;
+
+  /// Whether the associated RViz maker needs to be updated.
+  bool mNeedUpdate;
+
+  /// Whether the trajectory points need to be updated.
+  bool mNeedPointsUpdate;
+};
+
+} // namespace rviz
+} // namespace aikido
+
+#endif // AIKIDO_RVIZ_TRAJECTORYMARKER_HPP_

include/aikido/rviz/shape_conversions.hpp

@@ -34,6 +34,7 @@ geometry_msgs::Quaternion convertEigenToROSQuaternion(
     const Eigen::Quaterniond& v);
 geometry_msgs::Pose convertEigenToROSPose(const Eigen::Isometry3d& v);
 std_msgs::ColorRGBA convertEigenToROSColorRGBA(const Eigen::Vector4d& v);
+Eigen::Vector4d convertROSColorRGBAToEigen(const std_msgs::ColorRGBA& v);
 
 bool convertAssimpMeshToROSTriangleList(
     const aiMesh& mesh, std::vector<geometry_msgs::Point>* triangle_list);

src/planner/vectorfield/detail/VectorFieldIntegrator.cpp

@@ -153,7 +153,7 @@ void VectorFieldIntegrator::check(const Eigen::VectorXd& q, double t)
   }
 }
 
-} // namesapce detail
+} // namespace detail
 } // namespace vectorfield
 } // namespace planner
 } // namespace aikido

src/rviz/CMakeLists.txt

@@ -32,6 +32,7 @@ set(sources
   ShapeFrameMarker.cpp
   SkeletonMarker.cpp
   shape_conversions.cpp
+  TrajectoryMarker.cpp
   TSRMarker.cpp
   WorldInteractiveMarkerViewer.cpp
 )

src/rviz/InteractiveMarkerViewer.cpp

@@ -4,6 +4,7 @@
 #include <dart/dart.hpp>
 #include <aikido/rviz/FrameMarker.hpp>
 #include <aikido/rviz/SkeletonMarker.hpp>
+#include <aikido/rviz/TrajectoryMarker.hpp>
 
 using dart::dynamics::Skeleton;
 using dart::dynamics::SkeletonPtr;
@@ -16,11 +17,12 @@ namespace rviz {
 InteractiveMarkerViewer::InteractiveMarkerViewer(
     const std::string& topicNamespace, const std::string& frameId)
   : mMarkerServer(topicNamespace, "", true)
+  , mTrajectoryNameManager("Trajectory Name Manager", "Trajectory")
   , mRunning(false)
   , mUpdating(false)
   , mFrameId(frameId)
 {
-  // Do nothing
+  mTrajectoryNameManager.setPattern("Frame[%s(%d)]");
 }
 
 //==============================================================================
@@ -104,6 +106,32 @@ TSRMarkerPtr InteractiveMarkerViewer::addTSRMarker(
   return tsrMarker;
 }
 
+//==============================================================================
+TrajectoryMarkerPtr InteractiveMarkerViewer::addTrajectoryMarker(
+    trajectory::ConstTrajectoryPtr trajectory,
+    const dart::dynamics::MetaSkeleton& skeleton,
+    const dart::dynamics::Frame& frame,
+    const Eigen::Vector4d& rgba,
+    double thickness,
+    std::size_t numLineSegments)
+{
+  std::lock_guard<std::mutex> lock(mMutex);
+  DART_UNUSED(lock);
+  auto marker = std::make_shared<TrajectoryMarker>(
+      &mMarkerServer,
+      mFrameId,
+      mTrajectoryNameManager.issueNewNameAndAdd("", trajectory),
+      std::move(trajectory),
+      skeleton,
+      frame,
+      rgba,
+      thickness,
+      numLineSegments);
+  mTrajectoryMarkers.insert(marker);
+
+  return marker;
+}
+
 //==============================================================================
 SkeletonMarkerPtr InteractiveMarkerViewer::CreateSkeletonMarker(
     const SkeletonPtr& skeleton, const std::string& frameId)
@@ -166,6 +194,9 @@ void InteractiveMarkerViewer::update()
   for (const FrameMarkerPtr& marker : mFrameMarkers)
     marker->update();
 
+  for (const auto& marker : mTrajectoryMarkers)
+    marker->update();
+
   mMarkerServer.applyChanges();
 }