Fixed handle jumps when switching fixed frame in Rviz

cartesian_controller_handles/src/motion_control_handle.cpp

@@ -89,6 +89,14 @@ controller_interface::return_type MotionControlHandle::update(const rclcpp::Time
 controller_interface::return_type MotionControlHandle::update()
 #endif
 {
+  if (m_current_pose.header.frame_id != m_robot_base_link && m_current_pose.header.frame_id != "")
+  {
+    RCLCPP_ERROR(get_node()->get_logger(),
+                 "The reference frame \"%s\" selected in Rviz is not a fixed frame. Please select a "
+                 "fixed frame.",
+                 m_current_pose.header.frame_id.c_str());
+    return controller_interface::return_type::ERROR;
+  }
   // Publish marker pose
   m_current_pose.header.stamp = get_node()->now();
   m_current_pose.header.frame_id = m_robot_base_link;
@@ -248,12 +256,13 @@ void MotionControlHandle::updateMotionControlCallback(
   const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback)
 {
   // Move marker in RViz
-  m_server->setPose(feedback->marker_name, feedback->pose);
+  m_server->setPose(feedback->marker_name, feedback->pose, feedback->header);
   m_server->applyChanges();
 
   // Store for later broadcasting
   m_current_pose.pose = feedback->pose;
-  m_current_pose.header.stamp = get_node()->now();
+  m_current_pose.header.frame_id = feedback->header.frame_id;
+  m_current_pose.header.stamp = feedback->header.stamp;
 }
 
 void MotionControlHandle::updateMarkerMenuCallback(