Fix object interactive marker in wrong pose after changing the fixed …

…frame (#680)
moveit · Feb 7, 2022 · d7a3504 · d7a3504
1 parent 5244bc7
commit d7a3504
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 6 deletions.
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
@@ -1409,6 +1409,9 @@ void MotionPlanningDisplay::fixedFrameChanged()
   PlanningSceneDisplay::fixedFrameChanged();
   if (int_marker_display_)
     int_marker_display_->setFixedFrame(fixed_frame_);
+  // When the fixed frame changes we need to tell RViz to update the rendered interactive marker display
+  frame_->scene_marker_->requestPoseUpdate(frame_->scene_marker_->getPosition(),
+                                           frame_->scene_marker_->getOrientation());
   changedPlanningGroup();
 }
 

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
@@ -441,21 +441,29 @@ void MotionPlanningFrame::collisionObjectChanged(QListWidgetItem* item)
 /* Receives feedback from the interactive marker and updates the shape pose in the world accordingly */
 void MotionPlanningFrame::imProcessFeedback(visualization_msgs::msg::InteractiveMarkerFeedback& feedback)
 {
+  if (!planning_display_->getPlanningSceneRO()->knowsFrameTransform(feedback.header.frame_id))
+  {
+    RCLCPP_ERROR_STREAM(LOGGER,
+                        "Frame `" << feedback.header.frame_id << "` unknown doesn't exists in the planning scene");
+  }
+  Eigen::Isometry3d fixed_frame_t_scene_marker;
+  tf2::fromMsg(feedback.pose, fixed_frame_t_scene_marker);
+  Eigen::Isometry3d model_frame_t_scene_marker =
+      planning_display_->getPlanningSceneRO()->getFrameTransform(feedback.header.frame_id) * fixed_frame_t_scene_marker;
+
   bool old_state = ui_->object_x->blockSignals(true);
-  ui_->object_x->setValue(feedback.pose.position.x);
+  ui_->object_x->setValue(model_frame_t_scene_marker.translation().x());
   ui_->object_x->blockSignals(old_state);
 
   old_state = ui_->object_y->blockSignals(true);
-  ui_->object_y->setValue(feedback.pose.position.y);
+  ui_->object_y->setValue(model_frame_t_scene_marker.translation().y());
   ui_->object_y->blockSignals(old_state);
 
   old_state = ui_->object_z->blockSignals(true);
-  ui_->object_z->setValue(feedback.pose.position.z);
+  ui_->object_z->setValue(model_frame_t_scene_marker.translation().z());
   ui_->object_z->blockSignals(old_state);
 
-  Eigen::Quaterniond q;
-  tf2::fromMsg(feedback.pose.orientation, q);
-  Eigen::Vector3d xyz = q.matrix().eulerAngles(0, 1, 2);
+  Eigen::Vector3d xyz = model_frame_t_scene_marker.linear().eulerAngles(0, 1, 2);
 
   old_state = ui_->object_rx->blockSignals(true);
   ui_->object_rx->setValue(xyz[0]);