Merge branch 'moveit:main' into main

idesign0 · web-flow · commit 3eea6c6dcdd7 · 2025-12-10T13:26:56.000+01:00
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp
@@ -103,7 +103,8 @@ void TrajectoryGeneratorPolyline::extractMotionPlanInfo(const planning_scene::Pl
   for (const auto& pc : req.path_constraints.position_constraints)
   {
     Eigen::Isometry3d waypoint;
-    tf2::fromMsg(pc.constraint_region.primitive_poses.front(), waypoint);
+    waypoint = getConstraintPose(pc.constraint_region.primitive_poses.front().position,
+                                 pc.constraint_region.primitive_poses.front().orientation, pc.target_point_offset);
     waypoint = scene->getFrameTransform(frame_id) * waypoint;
     info.waypoints.push_back(waypoint);
   }

Original file line number	Diff line number	Diff line change
`@@ -103,7 +103,8 @@ void TrajectoryGeneratorPolyline::extractMotionPlanInfo(const planning_scene::Pl`
`103`	`103`	`for (const auto& pc : req.path_constraints.position_constraints)`
`104`	`104`	`{`
`105`	`105`	`Eigen::Isometry3d waypoint;`
`106`		`- tf2::fromMsg(pc.constraint_region.primitive_poses.front(), waypoint);`
	`106`	`+ waypoint = getConstraintPose(pc.constraint_region.primitive_poses.front().position,`
	`107`	`+ pc.constraint_region.primitive_poses.front().orientation, pc.target_point_offset);`
`107`	`108`	`waypoint = scene->getFrameTransform(frame_id) * waypoint;`
`108`	`109`	`info.waypoints.push_back(waypoint);`
`109`	`110`	`}`