GeneratePose: forward registered properties from received solution

This commit is contained in:
Robert Haschke 2023-01-28 01:06:21 +01:00
parent d9cff5510f
commit 93b46ef214

View File

@ -70,7 +70,9 @@ void GeneratePose::compute() {
if (upstream_solutions_.empty()) if (upstream_solutions_.empty())
return; 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"); geometry_msgs::PoseStamped target_pose = properties().get<geometry_msgs::PoseStamped>("pose");
if (target_pose.header.frame_id.empty()) if (target_pose.header.frame_id.empty())
target_pose.header.frame_id = scene->getPlanningFrame(); target_pose.header.frame_id = scene->getPlanningFrame();
@ -80,6 +82,7 @@ void GeneratePose::compute() {
} }
InterfaceState state(scene); InterfaceState state(scene);
forwardProperties(*s.end(), state); // forward registered properties from received solution
state.properties().set("target_pose", target_pose); state.properties().set("target_pose", target_pose);
SubTrajectory trajectory; SubTrajectory trajectory;