From 4e163eb3d715133a2191cc6b011c6136bfcee7eb Mon Sep 17 00:00:00 2001 From: marqrazz Date: Tue, 24 Dec 2024 09:03:11 -0700 Subject: [PATCH] Fix printing joint state by name --- moveit_core/planning_scene/src/planning_scene.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 4e8e105bc7..0cb2398ab6 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -629,8 +629,7 @@ moveit::core::RobotState& PlanningScene::getCurrentStateNonConst() robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } robot_state_.value().update(); - RCLCPP_INFO(getLogger(), "getCurrentStateNonConst(), joint 1: %f", robot_state_.value().getVariablePosition(0)); - // RCLCPP_INFO(getLogger(), "getCurrentStateNonConst, joint 1: %f", robot_state_.value().getJointPositions("panda_joint1")); + RCLCPP_INFO(getLogger(), "getCurrentStateNonConst, joint 1: %f", *robot_state_.value().getJointPositions("panda_joint1")); return robot_state_.value(); } @@ -1559,7 +1558,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } robot_state_.value().update(); - RCLCPP_WARN(getLogger(), "Updated robot_state_ has joint 1: %f", robot_state_.value().getVariablePosition(0)); + RCLCPP_WARN(getLogger(), "Updated robot_state_ has joint 1: %f", *robot_state_.value().getJointPositions("panda_joint1")); // The ADD/REMOVE operations follow this order: // STEP 1: Get info about the object from either the message or the world/RobotState