update PlanningScene's RobotState before storing it

This commit is contained in:
Robert Haschke 2018-10-20 22:02:05 +02:00
parent e894d8bce2
commit a0a9738a00
4 changed files with 13 additions and 5 deletions

View File

@ -98,7 +98,7 @@ public:
typedef std::deque<SolutionBase*> Solutions;
/// create an InterfaceState from a planning scene
InterfaceState(const planning_scene::PlanningSceneConstPtr& ps);
InterfaceState(const planning_scene::PlanningScenePtr& ps);
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories
InterfaceState(const InterfaceState& other);

View File

@ -112,7 +112,7 @@ void GenerateGraspPose::onNewSolution(const SolutionBase& s)
void GenerateGraspPose::compute() {
if (scenes_.empty())
return;
planning_scene::PlanningSceneConstPtr scene = scenes_[0];
planning_scene::PlanningScenePtr scene = scenes_[0];
scenes_.pop_front();
const auto& props = properties();

View File

@ -67,7 +67,7 @@ bool GeneratePose::canCompute() const {
void GeneratePose::compute() {
if (scenes_.empty())
return;
planning_scene::PlanningSceneConstPtr scene = scenes_[0];
planning_scene::PlanningScenePtr scene = scenes_[0];
scenes_.pop_front();
geometry_msgs::PoseStamped target_pose = properties().get<geometry_msgs::PoseStamped>("pose");

View File

@ -45,8 +45,16 @@
namespace moveit { namespace task_constructor {
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps)
: scene_(ps)
const planning_scene::PlanningScenePtr&
ensureUpdated(const planning_scene::PlanningScenePtr& scene) {
// ensure scene's state is updated
if (scene->getCurrentState().dirty())
scene->getCurrentStateNonConst().update();
return scene;
}
InterfaceState::InterfaceState(const planning_scene::PlanningScenePtr& ps)
: scene_(ensureUpdated(ps))
{
}