Scene: Fix setCollisionScene

- Addresses and resolves #149 - Broken in #125
ipab-slmc · Oct 16, 2017 · a266da0 · a266da0
1 parent 28b0f2d
commit a266da0
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 6 deletions.
diff --git a/exotica/include/exotica/Scene.h b/exotica/include/exotica/Scene.h
@@ -65,7 +65,7 @@ class Scene : public Object, Uncopyable, public Instantiable<SceneInitializer>
     std::shared_ptr<KinematicResponse> RequestKinematics(KinematicsRequest& Request);
     std::string getName();
     virtual void Update(Eigen::VectorXdRefConst x, double t = 0);
-    void setCollisionScene(const moveit_msgs::PlanningSceneConstPtr& scene);
+    void setCollisionScene(const moveit_msgs::PlanningScene& scene);
     CollisionScene_ptr& getCollisionScene();
     std::string getRootFrameName();
     std::string getRootJointName();

diff --git a/exotica/src/Scene.cpp b/exotica/src/Scene.cpp
@@ -225,8 +225,9 @@ visualization_msgs::Marker Scene::proxyToMarker(const std::vector<CollisionProxy
     return ret;
 }
 
-void Scene::setCollisionScene(const moveit_msgs::PlanningSceneConstPtr& scene)
+void Scene::setCollisionScene(const moveit_msgs::PlanningScene& scene)
 {
+    ps_->usePlanningSceneMsg(scene);
     updateSceneFrames();
     collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap());
 }

diff --git a/exotica_python/src/Exotica.cpp b/exotica_python/src/Exotica.cpp
@@ -599,10 +599,7 @@ PYBIND11_MODULE(_pyexotica, module)
     scene.def("setModelStateMap", (void (Scene::*)(std::map<std::string, double>, double)) & Scene::setModelState, py::arg("x"), py::arg("t") = 0.0);
     scene.def("publishScene", &Scene::publishScene);
     scene.def("publishProxies", &Scene::publishProxies);
-    scene.def("setCollisionScene", [](Scene* instance, moveit_msgs::PlanningScene& ps) {
-        moveit_msgs::PlanningSceneConstPtr myPtr(new moveit_msgs::PlanningScene(ps));
-        instance->setCollisionScene(myPtr);
-    });
+    scene.def("setCollisionScene", &Scene::setCollisionScene);
     scene.def("loadScene", &Scene::loadScene, py::arg("sceneString"), py::arg("updateCollisionScene") = true);
     scene.def("loadSceneFile", &Scene::loadSceneFile, py::arg("fileName"), py::arg("updateCollisionScene") = true);
     scene.def("getScene", &Scene::getScene);