diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index 8315bc95..5a0f033d 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -70,7 +70,9 @@ void GeneratePose::compute() { if (upstream_solutions_.empty()) return; - planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); + const SolutionBase& s = *upstream_solutions_.pop(); + planning_scene::PlanningSceneConstPtr scene = s.end()->scene()->diff(); + geometry_msgs::PoseStamped target_pose = properties().get("pose"); if (target_pose.header.frame_id.empty()) target_pose.header.frame_id = scene->getPlanningFrame(); @@ -80,6 +82,7 @@ void GeneratePose::compute() { } InterfaceState state(scene); + forwardProperties(*s.end(), state); // forward registered properties from received solution state.properties().set("target_pose", target_pose); SubTrajectory trajectory;