From 93b46ef21429bc91c8afa1b79479d302764b1e9d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 28 Jan 2023 01:06:21 +0100 Subject: [PATCH] GeneratePose: forward registered properties from received solution --- core/src/stages/generate_pose.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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;