GeneratePose: forward registered properties from received solution

Author: rhaschke

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<geometry_msgs::PoseStamped>("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;