diff --git a/CHANGELOG.md b/CHANGELOG.md index 2cd15dca45..5cf6419922 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/include/aikido/rviz/InteractiveMarkerViewer.hpp b/include/aikido/rviz/InteractiveMarkerViewer.hpp index 28d1643347..2ae39692ab 100644 --- a/include/aikido/rviz/InteractiveMarkerViewer.hpp +++ b/include/aikido/rviz/InteractiveMarkerViewer.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -13,6 +14,7 @@ #include #include #include +#include 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 mSkeletonMarkers; std::set mFrameMarkers; + std::set mTrajectoryMarkers; + + /// NameManager for name uniqueness of trajectories in the same + /// InteractiveMarkerServer. + dart::common::NameManager + mTrajectoryNameManager; std::atomic_bool mRunning; std::atomic_bool mUpdating; diff --git a/include/aikido/rviz/SmartPointers.hpp b/include/aikido/rviz/SmartPointers.hpp index 6bfb947544..25844108dc 100644 --- a/include/aikido/rviz/SmartPointers.hpp +++ b/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 diff --git a/include/aikido/rviz/TrajectoryMarker.hpp b/include/aikido/rviz/TrajectoryMarker.hpp new file mode 100644 index 0000000000..9f32575990 --- /dev/null +++ b/include/aikido/rviz/TrajectoryMarker.hpp @@ -0,0 +1,143 @@ +#ifndef AIKIDO_RVIZ_TRAJECTORYMARKER_HPP_ +#define AIKIDO_RVIZ_TRAJECTORYMARKER_HPP_ + +#include +#include +#include +#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_ diff --git a/include/aikido/rviz/shape_conversions.hpp b/include/aikido/rviz/shape_conversions.hpp index 3480c5b75a..c71bc6430b 100644 --- a/include/aikido/rviz/shape_conversions.hpp +++ b/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* triangle_list); diff --git a/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp b/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp index e3e26789a5..f97325975a 100644 --- a/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp +++ b/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 diff --git a/src/rviz/CMakeLists.txt b/src/rviz/CMakeLists.txt index dc9aa70490..793e81a15a 100644 --- a/src/rviz/CMakeLists.txt +++ b/src/rviz/CMakeLists.txt @@ -32,6 +32,7 @@ set(sources ShapeFrameMarker.cpp SkeletonMarker.cpp shape_conversions.cpp + TrajectoryMarker.cpp TSRMarker.cpp WorldInteractiveMarkerViewer.cpp ) diff --git a/src/rviz/InteractiveMarkerViewer.cpp b/src/rviz/InteractiveMarkerViewer.cpp index 004e755ac1..fbb1bb1657 100644 --- a/src/rviz/InteractiveMarkerViewer.cpp +++ b/src/rviz/InteractiveMarkerViewer.cpp @@ -4,6 +4,7 @@ #include #include #include +#include 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 lock(mMutex); + DART_UNUSED(lock); + auto marker = std::make_shared( + &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(); } diff --git a/src/rviz/TrajectoryMarker.cpp b/src/rviz/TrajectoryMarker.cpp new file mode 100644 index 0000000000..3c6c96ac23 --- /dev/null +++ b/src/rviz/TrajectoryMarker.cpp @@ -0,0 +1,269 @@ +#include "aikido/rviz/TrajectoryMarker.hpp" + +#include "aikido/rviz/shape_conversions.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpaceSaver.hpp" + +namespace aikido { +namespace rviz { + +//============================================================================== +TrajectoryMarker::TrajectoryMarker( + interactive_markers::InteractiveMarkerServer* markerServer, + const std::string& frameId, + const std::string& markerName, + trajectory::ConstTrajectoryPtr trajectory, + const dart::dynamics::MetaSkeleton& targetSkeleton, + const dart::dynamics::Frame& frame, + const Eigen::Vector4d& rgba, + double thickness, + std::size_t numLineSegments) + : mMarkerServer(markerServer) + , mInteractiveMarker() + , mFrameId(frameId) + , mTrajectory(nullptr) + , mNumLineSegments() + , mSkeleton(targetSkeleton) + , mFrame(frame) + , mNeedUpdate(true) + , mNeedPointsUpdate(true) +{ + using visualization_msgs::InteractiveMarkerControl; + using visualization_msgs::Marker; + + // Setting invariant properties + mInteractiveMarker.header.frame_id = mFrameId; + mInteractiveMarker.name = markerName; + // TODO: Assign trajectory name once available + mInteractiveMarker.pose.orientation.w = 1; + mInteractiveMarker.scale = 1; + + mInteractiveMarker.controls.resize(1); + InteractiveMarkerControl& control = mInteractiveMarker.controls.front(); + control.orientation.w = 1; + control.orientation_mode = InteractiveMarkerControl::INHERIT; + control.interaction_mode = InteractiveMarkerControl::NONE; + control.always_visible = true; + control.markers.resize(1); + + auto& marker = getMarker(); + marker.type = Marker::LINE_STRIP; + marker.pose.orientation.w = 1.0; + + // Setting variant properties + setTrajectory(std::move(trajectory)); + setRGBA(rgba); + setThickness(thickness); + setNumLineSegments(numLineSegments); +} + +//============================================================================== +TrajectoryMarker::~TrajectoryMarker() +{ + mMarkerServer->erase(mInteractiveMarker.name); +} + +//============================================================================== +void TrajectoryMarker::setTrajectory(trajectory::ConstTrajectoryPtr trajectory) +{ + using visualization_msgs::Marker; + + if (trajectory) + { + auto statespace = trajectory->getStateSpace(); + if (!std::dynamic_pointer_cast( + statespace)) + { + throw std::invalid_argument( + "The statespace in the trajectory should be MetaSkeletonStateSpace"); + } + + if (statespace->getDimension() != mSkeleton.getNumDofs()) + { + throw std::invalid_argument( + "The statespace in the trajectory is not compatible (dimensions are " + "different) to DART meta skeleton for visualizing the trajectory"); + } + // TODO: Use more comprehensive compatibility check once available than + // just checking the dimensions. + } + + mTrajectory = std::move(trajectory); + + mNeedPointsUpdate = true; +} + +//============================================================================== +trajectory::ConstTrajectoryPtr TrajectoryMarker::getTrajectory() const +{ + return mTrajectory; +} + +//============================================================================== +void TrajectoryMarker::setColor(const Eigen::Vector3d& rgb) +{ + auto& marker = getMarker(); + marker.color.r = rgb[0]; + marker.color.g = rgb[1]; + marker.color.b = rgb[2]; + + mNeedUpdate = true; +} + +//============================================================================== +Eigen::Vector3d TrajectoryMarker::getColor() const +{ + const auto& marker = getMarker(); + return convertROSColorRGBAToEigen(marker.color).head<3>(); +} + +//============================================================================== +void TrajectoryMarker::setAlpha(double alpha) +{ + auto& marker = getMarker(); + marker.color.a = alpha; + + mNeedUpdate = true; +} + +//============================================================================== +double TrajectoryMarker::getAlpha() const +{ + const auto& marker = getMarker(); + return marker.color.a; +} + +//============================================================================== +void TrajectoryMarker::setRGBA(const Eigen::Vector4d& rgba) +{ + auto& marker = getMarker(); + marker.color = convertEigenToROSColorRGBA(rgba); + + mNeedUpdate = true; +} + +//============================================================================== +Eigen::Vector4d TrajectoryMarker::getRBGA() const +{ + const auto& marker = getMarker(); + return convertROSColorRGBAToEigen(marker.color); +} + +//============================================================================== +void TrajectoryMarker::setThickness(double thickness) +{ + auto& marker = getMarker(); + marker.scale.x = thickness; + + mNeedUpdate = true; +} + +//============================================================================== +double TrajectoryMarker::getThickness() const +{ + const auto& marker = getMarker(); + return marker.scale.x; +} + +//============================================================================== +void TrajectoryMarker::setNumLineSegments(std::size_t numLineSegments) +{ + if (numLineSegments == 0u) + { + dtwarn << "[TrajectoryMarker::setNumLineSegments] numLineSegments is set to" + "zero. This trajectory will not be rendered."; + } + + mNumLineSegments = numLineSegments; + + mNeedPointsUpdate = true; +} + +//============================================================================== +std::size_t TrajectoryMarker::getNumLineSegments() const +{ + return mNumLineSegments; +} + +//============================================================================== +void TrajectoryMarker::update() +{ + if (mNeedPointsUpdate) + updatePoints(); + + if (!mNeedUpdate) + return; + + mMarkerServer->insert(mInteractiveMarker); + + mNeedUpdate = false; +} + +//============================================================================== +void TrajectoryMarker::updatePoints() +{ + using visualization_msgs::Marker; + + if (!mNeedPointsUpdate) + return; + + auto& marker = getMarker(); + auto& points = marker.points; + points.clear(); + + if (!mTrajectory || mNumLineSegments == 0u) + { + mNeedPointsUpdate = false; + mNeedUpdate = true; + return; + } + + statespace::StateSpacePtr statespace = mTrajectory->getStateSpace(); + auto metaSkeletonSs + = std::dynamic_pointer_cast( + statespace); + + auto saver = statespace::dart::MetaSkeletonStateSpaceSaver(metaSkeletonSs); + DART_UNUSED(saver); + + auto state = metaSkeletonSs->createState(); + double t = mTrajectory->getStartTime(); + const double dt = mTrajectory->getDuration() / mNumLineSegments; + + points.reserve(mNumLineSegments + 1u); + Eigen::Vector3d pose; + for (std::size_t i = 0u; i < mNumLineSegments - 1u; ++i) + { + mTrajectory->evaluate(t, state); + metaSkeletonSs->setState(state); + pose = mFrame.getTransform().translation(); + points.emplace_back(convertEigenToROSPoint(pose)); + t += dt; + } + mTrajectory->evaluate(mTrajectory->getEndTime(), state); + metaSkeletonSs->setState(state); + pose = mFrame.getTransform().translation(); + points.emplace_back(convertEigenToROSPoint(pose)); + + mNeedPointsUpdate = false; + + mNeedUpdate = true; +} + +//============================================================================== +visualization_msgs::Marker& TrajectoryMarker::getMarker() +{ + using visualization_msgs::Marker; + Marker& marker = mInteractiveMarker.controls.front().markers.front(); + return marker; +} + +//============================================================================== +const visualization_msgs::Marker& TrajectoryMarker::getMarker() const +{ + auto& marker = const_cast(this)->getMarker(); + return const_cast(marker); +} + +} // namespace rviz +} // namespace aikido diff --git a/src/rviz/WorldInteractiveMarkerViewer.cpp b/src/rviz/WorldInteractiveMarkerViewer.cpp index 48a2bb9325..b633745f76 100644 --- a/src/rviz/WorldInteractiveMarkerViewer.cpp +++ b/src/rviz/WorldInteractiveMarkerViewer.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include using aikido::rviz::InteractiveMarkerViewer; @@ -92,6 +93,9 @@ void WorldInteractiveMarkerViewer::update() for (const FrameMarkerPtr& marker : mFrameMarkers) marker->update(); + for (const auto& marker : mTrajectoryMarkers) + marker->update(); + mMarkerServer.applyChanges(); } diff --git a/src/rviz/shape_conversions.cpp b/src/rviz/shape_conversions.cpp index 15bd88717e..42ec054600 100644 --- a/src/rviz/shape_conversions.cpp +++ b/src/rviz/shape_conversions.cpp @@ -50,6 +50,17 @@ std_msgs::ColorRGBA convertEigenToROSColorRGBA(const Eigen::Vector4d& v) return v_ros; } +//============================================================================== +Eigen::Vector4d convertROSColorRGBAToEigen(const std_msgs::ColorRGBA& v) +{ + Eigen::Vector4d vec4; + vec4[0] = v.r; + vec4[1] = v.g; + vec4[2] = v.b; + vec4[3] = v.a; + return vec4; +} + //============================================================================== geometry_msgs::Quaternion convertEigenToROSQuaternion( const Eigen::Quaterniond& v)