From a266da08fbb79c253c6610efa39b2dfc0b79efc9 Mon Sep 17 00:00:00 2001
From: Wolfgang Merkt <w.merkt@gmail.com>
Date: Mon, 16 Oct 2017 18:02:34 +0100
Subject: [PATCH] Scene: Fix setCollisionScene - Addresses and resolves #149 -
 Broken in #125

---
 exotica/include/exotica/Scene.h | 2 +-
 exotica/src/Scene.cpp           | 3 ++-
 exotica_python/src/Exotica.cpp  | 5 +----
 3 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/exotica/include/exotica/Scene.h b/exotica/include/exotica/Scene.h
index 75cfeaa8d3..9f9b435329 100644
--- 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
index 7e4e5b40e8..c0fbba4064 100644
--- 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
index a9a9ac5b70..d4eb561e86 100644
--- 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);