From 24565ee1ebbd489dbb24d03c9f080d37dff914a3 Mon Sep 17 00:00:00 2001 From: Davide Date: Sun, 2 Jun 2024 14:10:45 +0200 Subject: [PATCH 1/2] 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( From 6c5fbeca02d379de8237bca836d46b0edaaae425 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 18 Oct 2024 13:13:26 +0200 Subject: [PATCH 2/2] Clang-format previous changes --- .../src/motion_control_handle.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/cartesian_controller_handles/src/motion_control_handle.cpp b/cartesian_controller_handles/src/motion_control_handle.cpp index 1a1903a1..f29e404a 100644 --- a/cartesian_controller_handles/src/motion_control_handle.cpp +++ b/cartesian_controller_handles/src/motion_control_handle.cpp @@ -91,10 +91,11 @@ controller_interface::return_type MotionControlHandle::update() { 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()); + 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