From 1fac8bb04f462cfbac40f4aec8754caeb07de685 Mon Sep 17 00:00:00 2001 From: Pengju Jin Date: Wed, 26 Apr 2017 00:54:30 -0400 Subject: [PATCH] Add frame_id parameter to aikido:rviz (#183) --- include/aikido/rviz/BodyNodeMarker.hpp | 4 +++- include/aikido/rviz/FrameMarker.hpp | 2 ++ include/aikido/rviz/InteractiveMarkerViewer.hpp | 8 ++++++-- include/aikido/rviz/ShapeFrameMarker.hpp | 6 ++++-- include/aikido/rviz/SkeletonMarker.hpp | 4 +++- src/perception/AprilTagsModule.cpp | 2 -- src/rviz/BodyNodeMarker.cpp | 6 ++++-- src/rviz/FrameMarker.cpp | 4 +++- src/rviz/InteractiveMarkerViewer.cpp | 15 +++++++++------ src/rviz/ShapeFrameMarker.cpp | 6 ++++-- src/rviz/SkeletonMarker.cpp | 6 ++++-- 11 files changed, 42 insertions(+), 21 deletions(-) diff --git a/include/aikido/rviz/BodyNodeMarker.hpp b/include/aikido/rviz/BodyNodeMarker.hpp index 9f30f6bac5..880ceebae0 100644 --- a/include/aikido/rviz/BodyNodeMarker.hpp +++ b/include/aikido/rviz/BodyNodeMarker.hpp @@ -15,7 +15,8 @@ class BodyNodeMarker { BodyNodeMarker( ResourceServer *resourceServer, interactive_markers::InteractiveMarkerServer *markerServer, - dart::dynamics::WeakBodyNodePtr const &bodyNode); + dart::dynamics::WeakBodyNodePtr const &bodyNode, + const std::string &frameId); BodyNodeMarker(BodyNodeMarker const &) = delete; BodyNodeMarker &operator=(BodyNodeMarker const &) = delete; @@ -38,6 +39,7 @@ class BodyNodeMarker { interactive_markers::InteractiveMarkerServer *mMarkerServer; visualization_msgs::InteractiveMarker mInteractiveMarker; visualization_msgs::InteractiveMarkerControl *mVisualControl; + std::string mFrameId; std::string mName; ShapeFrameMarkerMap mShapeFrameMarkers; diff --git a/include/aikido/rviz/FrameMarker.hpp b/include/aikido/rviz/FrameMarker.hpp index c3f6d16b17..bdd639d1df 100644 --- a/include/aikido/rviz/FrameMarker.hpp +++ b/include/aikido/rviz/FrameMarker.hpp @@ -20,6 +20,7 @@ class FrameMarker { FrameMarker( interactive_markers::InteractiveMarkerServer *markerServer, dart::dynamics::Frame *frame, + const std::string& frameId, double length = 0.25, double thickness = 0.02, double alpha = 1.0); @@ -32,6 +33,7 @@ class FrameMarker { visualization_msgs::InteractiveMarker mInteractiveMarker; dart::dynamics::Frame *mFrame; + std::string mFrameId; }; } // namespace rviz diff --git a/include/aikido/rviz/InteractiveMarkerViewer.hpp b/include/aikido/rviz/InteractiveMarkerViewer.hpp index 48e6f8af94..f1c4a2526c 100644 --- a/include/aikido/rviz/InteractiveMarkerViewer.hpp +++ b/include/aikido/rviz/InteractiveMarkerViewer.hpp @@ -20,7 +20,8 @@ namespace rviz { class InteractiveMarkerViewer { public: - InteractiveMarkerViewer(const std::string& topicNamespace); + InteractiveMarkerViewer(const std::string& topicNamespace, + const std::string& frameId); virtual ~InteractiveMarkerViewer(); InteractiveMarkerViewer(const InteractiveMarkerViewer&) = delete; @@ -36,7 +37,8 @@ class InteractiveMarkerViewer double thickness = 0.02, double alpha = 1.0); SkeletonMarkerPtr CreateSkeletonMarker( - const dart::dynamics::SkeletonPtr& skeleton); + const dart::dynamics::SkeletonPtr& skeleton, + const std::string& frameId); /// Visualizes a TSR. /// \param tsr TSR constraint @@ -62,6 +64,8 @@ class InteractiveMarkerViewer std::atomic_bool mUpdating; std::mutex mMutex; std::thread mThread; + + std::string mFrameId; }; } // namespace rviz diff --git a/include/aikido/rviz/ShapeFrameMarker.hpp b/include/aikido/rviz/ShapeFrameMarker.hpp index 0318d6b695..a6f19d56fd 100644 --- a/include/aikido/rviz/ShapeFrameMarker.hpp +++ b/include/aikido/rviz/ShapeFrameMarker.hpp @@ -17,7 +17,8 @@ class ShapeFrameMarker ResourceServer *resourceServer, interactive_markers::InteractiveMarkerServer *markerServer, const std::string &name, - const dart::dynamics::ShapeFrame *shapeFrame); + const dart::dynamics::ShapeFrame *shapeFrame, + const std::string &frameId); ShapeFrameMarker(ShapeFrameMarker const &) = delete; ShapeFrameMarker &operator=(ShapeFrameMarker const &) = delete; @@ -36,7 +37,8 @@ class ShapeFrameMarker visualization_msgs::InteractiveMarkerControl *mVisualControl; const dart::dynamics::ShapeFrame *mShapeFrame; - + std::string mFrameId; + bool mExists; bool mForceUpdate; size_t mVersion; diff --git a/include/aikido/rviz/SkeletonMarker.hpp b/include/aikido/rviz/SkeletonMarker.hpp index 08aae1ed87..e495a7007c 100644 --- a/include/aikido/rviz/SkeletonMarker.hpp +++ b/include/aikido/rviz/SkeletonMarker.hpp @@ -16,7 +16,8 @@ class SkeletonMarker { SkeletonMarker( ResourceServer *resourceServer, interactive_markers::InteractiveMarkerServer *markerServer, - dart::dynamics::WeakSkeletonPtr const &skeleton); + dart::dynamics::WeakSkeletonPtr const &skeleton, + const std::string &frameId); dart::dynamics::SkeletonPtr getSkeleton() const; std::vector bodynode_markers() const; @@ -37,6 +38,7 @@ class SkeletonMarker { BodyNodeMarkerPtr> mBodyNodeMarkers; bool mHasColor; + std::string mFrameId; Eigen::Vector4d mColor; }; diff --git a/src/perception/AprilTagsModule.cpp b/src/perception/AprilTagsModule.cpp index 873756f4b9..e28c6ed15c 100644 --- a/src/perception/AprilTagsModule.cpp +++ b/src/perception/AprilTagsModule.cpp @@ -116,7 +116,6 @@ bool AprilTagsModule::detectObjects(const dart::simulation::WorldPtr& env, ros:: if(env_skeleton == nullptr){ // Getting the model for the new object - std::cout << "getting new thing" << std::endl; is_new_skel = true; dart::utils::DartLoader urdfLoader; skel_to_update = @@ -157,7 +156,6 @@ bool AprilTagsModule::detectObjects(const dart::simulation::WorldPtr& env, ros:: if(is_new_skel){ //Adding new skeleton to the world env env->addSkeleton(skel_to_update); - std::cout << "added new thing" << std::endl; } } diff --git a/src/rviz/BodyNodeMarker.cpp b/src/rviz/BodyNodeMarker.cpp index cbcc1e7846..c39e6a0a62 100644 --- a/src/rviz/BodyNodeMarker.cpp +++ b/src/rviz/BodyNodeMarker.cpp @@ -14,10 +14,12 @@ using interactive_markers::InteractiveMarkerServer; BodyNodeMarker::BodyNodeMarker(ResourceServer *resourceServer, InteractiveMarkerServer *markerServer, - WeakBodyNodePtr const &bodyNodeWeak) + WeakBodyNodePtr const &bodyNodeWeak, + const std::string &frameId) : mBodyNode(bodyNodeWeak) , mResourceServer(resourceServer) , mMarkerServer(markerServer) + , mFrameId(frameId) { // Register callbacks on BodyNode changes. BodyNodePtr const bodyNode = mBodyNode.lock(); @@ -66,7 +68,7 @@ bool BodyNodeMarker::update() mShapeFrameMarkers.emplace(shapeNode, std::unique_ptr( new ShapeFrameMarker(mResourceServer, mMarkerServer, shapeNodeName.str(), - shapeNode))); + shapeNode, mFrameId))); } // Update all of the ShapeFrameMarkers. diff --git a/src/rviz/FrameMarker.cpp b/src/rviz/FrameMarker.cpp index f01b8d3f82..f3139bb3fc 100644 --- a/src/rviz/FrameMarker.cpp +++ b/src/rviz/FrameMarker.cpp @@ -42,13 +42,15 @@ static void CreateAxis( FrameMarker::FrameMarker( interactive_markers::InteractiveMarkerServer *markerServer, dart::dynamics::Frame *frame, + const std::string& frameId, double length, double thickness, double alpha) : mMarkerServer(markerServer) , mFrame(frame) + , mFrameId(frameId) { using visualization_msgs::InteractiveMarkerControl; - mInteractiveMarker.header.frame_id = "map"; + mInteractiveMarker.header.frame_id = mFrameId; mInteractiveMarker.name = str(format("Frame[%s]") % frame->getName()); mInteractiveMarker.pose.orientation.w = 1; mInteractiveMarker.scale = 1; diff --git a/src/rviz/InteractiveMarkerViewer.cpp b/src/rviz/InteractiveMarkerViewer.cpp index 18d05bf8e3..f4809f594b 100644 --- a/src/rviz/InteractiveMarkerViewer.cpp +++ b/src/rviz/InteractiveMarkerViewer.cpp @@ -12,10 +12,11 @@ namespace aikido { namespace rviz { InteractiveMarkerViewer::InteractiveMarkerViewer( - const std::string& topicNamespace) + const std::string& topicNamespace, const std::string& frameId) : mMarkerServer(topicNamespace, "", true), mRunning(false), - mUpdating(false) + mUpdating(false), + mFrameId(frameId) { } @@ -34,7 +35,7 @@ SkeletonMarkerPtr InteractiveMarkerViewer::addSkeleton( const SkeletonPtr& skeleton) { std::lock_guard lock(mMutex); - const SkeletonMarkerPtr marker = CreateSkeletonMarker(skeleton); + const SkeletonMarkerPtr marker = CreateSkeletonMarker(skeleton, mFrameId); mSkeletonMarkers.insert(marker); return marker; } @@ -44,7 +45,7 @@ FrameMarkerPtr InteractiveMarkerViewer::addFrame( { std::lock_guard lock(mMutex); const FrameMarkerPtr marker = std::make_shared( - &mMarkerServer, frame, length, thickness, alpha); + &mMarkerServer, frame, mFrameId, length, thickness, alpha); mFrameMarkers.insert(marker); return marker; } @@ -97,11 +98,13 @@ TSRMarkerPtr InteractiveMarkerViewer::addTSRMarker( SkeletonMarkerPtr InteractiveMarkerViewer::CreateSkeletonMarker( - const SkeletonPtr& skeleton) + const SkeletonPtr& skeleton, const std::string& frameId) { - return std::make_shared(nullptr, &mMarkerServer, skeleton); + return std::make_shared( + nullptr, &mMarkerServer, skeleton, frameId); } + void InteractiveMarkerViewer::setAutoUpdate(bool flag) { mUpdating.store(flag, std::memory_order_release); diff --git a/src/rviz/ShapeFrameMarker.cpp b/src/rviz/ShapeFrameMarker.cpp index eabbf05f03..12fc092680 100644 --- a/src/rviz/ShapeFrameMarker.cpp +++ b/src/rviz/ShapeFrameMarker.cpp @@ -25,10 +25,12 @@ ShapeFrameMarker::ShapeFrameMarker( ResourceServer *resourceServer, InteractiveMarkerServer *markerServer, const std::string &name, - ShapeFrame const *shapeFrame) + ShapeFrame const *shapeFrame, + const std::string &frameId) : mResourceServer(resourceServer) , mMarkerServer(markerServer) , mShapeFrame(shapeFrame) + , mFrameId(frameId) , mExists(false) , mForceUpdate(true) , mVersion() @@ -38,7 +40,7 @@ ShapeFrameMarker::ShapeFrameMarker( using std::placeholders::_1; using std::placeholders::_2; - mInteractiveMarker.header.frame_id = "map"; + mInteractiveMarker.header.frame_id = mFrameId; mInteractiveMarker.scale = 1.; mInteractiveMarker.controls.resize(1); mInteractiveMarker.name = name; diff --git a/src/rviz/SkeletonMarker.cpp b/src/rviz/SkeletonMarker.cpp index dac5f332a2..822f5c3edf 100644 --- a/src/rviz/SkeletonMarker.cpp +++ b/src/rviz/SkeletonMarker.cpp @@ -11,11 +11,13 @@ using interactive_markers::InteractiveMarkerServer; SkeletonMarker::SkeletonMarker(ResourceServer *resourceServer, InteractiveMarkerServer *markerServer, - WeakSkeletonPtr const &skeleton) + WeakSkeletonPtr const &skeleton, + const std::string &frameId) : mSkeleton(skeleton) , mResourceServer(resourceServer) , mMarkerServer(markerServer) , mHasColor(false) + , mFrameId(frameId) { } @@ -39,7 +41,7 @@ bool SkeletonMarker::update() if (result.second) { bodyNodeMarker = std::make_shared( - mResourceServer, mMarkerServer, WeakBodyNodePtr(bodyNode)); + mResourceServer, mMarkerServer, WeakBodyNodePtr(bodyNode), mFrameId); if (mHasColor) bodyNodeMarker->SetColor(mColor);