mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
update PlanningScene's RobotState before storing it
This commit is contained in:
parent
e894d8bce2
commit
a0a9738a00
@ -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);
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -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))
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user