Maintain subframes when scaling/renaming objects in Rviz

moveit · Jul 4, 2020 · 04ec95d · 04ec95d
1 parent 525d4a3
commit 04ec95d
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
@@ -114,6 +114,7 @@ void MotionPlanningFrame::sceneScaleChanged(int value)
     {
       if (ps->getWorld()->hasObject(scaled_object_->id_))
       {
+        const moveit::core::FixedTransformsMap subframes = scaled_object_->subframe_poses_;  // Keep subframes
         ps->getWorldNonConst()->removeObject(scaled_object_->id_);
         for (std::size_t i = 0; i < scaled_object_->shapes_.size(); ++i)
         {
@@ -122,6 +123,7 @@ void MotionPlanningFrame::sceneScaleChanged(int value)
           ps->getWorldNonConst()->addToObject(scaled_object_->id_, shapes::ShapeConstPtr(s),
                                               scaled_object_->shape_poses_[i]);
         }
+        ps->getWorldNonConst()->setSubframesOfObject(scaled_object_->id_, subframes);
         scene_marker_->processMessage(createObjectMarkerMsg(ps->getWorld()->getObject(scaled_object_->id_)));
         planning_display_->queueRenderSceneGeometry();
       }
@@ -788,9 +790,11 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item)
     if (obj)
     {
       known_collision_objects_[item->type()].first = item_text;
+      const moveit::core::FixedTransformsMap subframes = obj->subframe_poses_;  // Keep subframes
       ps->getWorldNonConst()->removeObject(obj->id_);
       ps->getWorldNonConst()->addToObject(known_collision_objects_[item->type()].first, obj->shapes_,
                                           obj->shape_poses_);
+      ps->getWorldNonConst()->setSubframesOfObject(obj->id_, subframes);
       if (scene_marker_)
       {
         scene_marker_.reset();