From 17d8a9d2360ecdb476868d188fa994743db9534a Mon Sep 17 00:00:00 2001 From: Davide Date: Sun, 2 Jun 2024 14:10:45 +0200 Subject: [PATCH] Fixed handle jumps when switching fixed frame in Rviz --- .../src/motion_control_handle.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/cartesian_controller_handles/src/motion_control_handle.cpp b/cartesian_controller_handles/src/motion_control_handle.cpp index 18cc210f..1a1903a1 100644 --- a/cartesian_controller_handles/src/motion_control_handle.cpp +++ b/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(