planning scene debug [WIP]

moveit · Aug 19, 2024 · c891ed9 · c891ed9
1 parent f10ff7a
commit c891ed9
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp
@@ -272,6 +272,13 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin
               std::make_unique<GeometryInstance>(
                   convertToDrakeFrame(pose),
                   std::make_unique<Box>(objectptr->size[0], objectptr->size[1], objectptr->size[2]), shape_name));
+          RCLCPP_INFO(
+              getLogger(), 
+              "translation of the body x:%f, y:%f, z:%f",
+              pose.translation().x(),
+              pose.translation().y(),
+              pose.translation().z()
+            );
 
           // add illustration, proximity, perception properties
           scene_graph.AssignRole(box_source_id, box_geom_id, IllustrationProperties());