Add frame_id parameter to aikido:rviz (#183)

personalrobotics · Apr 26, 2017 · 1fac8bb · 1fac8bb
1 parent 797797f
commit 1fac8bb
Show file tree

Hide file tree

Showing 11 changed files with 42 additions and 21 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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<BodyNodeMarkerPtr> 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
@@ -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
@@ -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<ShapeFrameMarker>(
       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
@@ -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
@@ -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<std::mutex> 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<std::mutex> lock(mMutex);
   const FrameMarkerPtr marker = std::make_shared<FrameMarker>(
-    &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<SkeletonMarker>(nullptr, &mMarkerServer, skeleton);
+  return std::make_shared<SkeletonMarker>(
+    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
@@ -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
@@ -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<BodyNodeMarker>(
-        mResourceServer, mMarkerServer, WeakBodyNodePtr(bodyNode));
+        mResourceServer, mMarkerServer, WeakBodyNodePtr(bodyNode), mFrameId);
 
       if (mHasColor)
         bodyNodeMarker->SetColor(mColor);