Merge pull request #151 from ipab-slmc/wxm-fix-scene-updates

Fix scene updates from MoveIt messages, resolves #149
ipab-slmc · Oct 17, 2017 · 09f18a9 · 09f18a9
2 parents 262ba43 + a266da0
commit 09f18a9
Show file tree

Hide file tree

Showing 10 changed files with 86 additions and 28 deletions.
diff --git a/...s/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h b/...s/collision_scene_fcl_latest/include/collision_scene_fcl_latest/CollisionSceneFCLLatest.h
@@ -91,6 +91,7 @@ class CollisionSceneFCLLatest : public CollisionScene
     ///
     virtual std::vector<CollisionProxy> getCollisionDistance(bool self);
     virtual std::vector<CollisionProxy> getCollisionDistance(const std::string& o1, const std::string& o2);
+    virtual std::vector<CollisionProxy> getCollisionDistance(const std::string& o1);
 
     /**
        * @brief      Gets the collision world links.

diff --git a/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp b/exotations/collision_scene_fcl_latest/src/CollisionSceneFCLLatest.cpp
@@ -343,6 +343,54 @@ std::vector<CollisionProxy> CollisionSceneFCLLatest::getCollisionDistance(const
     return data.Proxies;
 }
 
+std::vector<CollisionProxy> CollisionSceneFCLLatest::getCollisionDistance(
+    const std::string& o1)
+{
+    std::vector<fcl::CollisionObjectd*> shapes1;
+    std::vector<fcl::CollisionObjectd*> shapes2;
+    DistanceData data(this);
+
+    // Iterate over all fcl_objects_ to find all collision links that belong to
+    // object o1
+    for (fcl::CollisionObjectd* o : fcl_objects_)
+    {
+        KinematicElement* e = reinterpret_cast<KinematicElement*>(o->getUserData());
+        if (e->Segment.getName() == o1 || e->Parent->Segment.getName() == o1)
+            shapes1.push_back(o);
+    }
+
+    // Iterate over all fcl_objects_ to find all objects o1 is allowed to collide
+    // with
+    for (fcl::CollisionObjectd* o : fcl_objects_)
+    {
+        KinematicElement* e = reinterpret_cast<KinematicElement*>(o->getUserData());
+        // Collision Object does not belong to o1
+        if (e->Segment.getName() != o1 || e->Parent->Segment.getName() != o1)
+        {
+            bool allowedToCollide = false;
+            for (fcl::CollisionObjectd* o1_shape : shapes1)
+                if (isAllowedToCollide(o1_shape, o, data.Self, data.Scene))
+                    allowedToCollide = true;
+
+            if (allowedToCollide) shapes2.push_back(o);
+        }
+    }
+
+    if (shapes1.size() == 0) throw_pretty("Can't find object '" << o1 << "'!");
+    if (shapes2.size() == 0)
+        throw_pretty("Can't find any objects '" << o1
+                                                << "' is allowed to collide with!");
+
+    for (fcl::CollisionObjectd* s1 : shapes1)
+    {
+        for (fcl::CollisionObjectd* s2 : shapes2)
+        {
+            computeDistance(s1, s2, &data);
+        }
+    }
+    return data.Proxies;
+}
+
 Eigen::Vector3d CollisionSceneFCLLatest::getTranslation(const std::string& name)
 {
     for (fcl::CollisionObjectd* object : fcl_objects_)

diff --git a/exotations/task_maps/task_map/CMakeLists.txt b/exotations/task_maps/task_map/CMakeLists.txt
@@ -1,8 +1,6 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
 project(task_map)
 
-add_definitions(-DEXOTICA_DEBUG_MODE)
-
 find_package(catkin REQUIRED COMPONENTS
   exotica
   exotica_python

diff --git a/exotica/doc/Python-API.rst b/exotica/doc/Python-API.rst
@@ -1,7 +1,13 @@
-**********
-Python API
-**********
+***************
+Python Bindings
+***************
+
+By default, the Python bindings will be compiled for 2.7. In order to build for different versions of Python you can specify ``PYBIND_PYTHON_EXECUTABLE`` in the additional CMake arguments of your catkin workspace:
+
+.. code:: bash
+
+    catkin config --cmake-args -DPYBIND_PYTHON_EXECUTABLE=/usr/bin/python3
 
 .. NB: This is deactivated/not included in the toctree until the segfault upon loading the Python module is resolved - this prevents auto-generation.
 
-.. automodule:: pyexotica
+.. automodule: : pyexotica
diff --git a/exotica/doc/index.rst b/exotica/doc/index.rst
@@ -43,12 +43,11 @@ Overview
    Quickstart
    Setting-up-ROSlaunch
    Using-EXOTica
+   Python-API
    Documentation
    Code-Formatting
    Doxygen C++ <./doxygen_cpp/index.html#http://>
 
-.. Python-API
-
 
 .. Indices and tables
     ==================

diff --git a/exotica/include/exotica/CollisionScene.h b/exotica/include/exotica/CollisionScene.h
@@ -103,6 +103,13 @@ class CollisionScene : public Uncopyable
     /// \return Vector of proximity objects.
     ///
     virtual std::vector<CollisionProxy> getCollisionDistance(const std::string& o1, const std::string& o2) { throw_pretty("Not implemented!"); }
+    /**
+   * @brief      Gets the closest distance of any collision object which is
+   * allowed to collide with any collision object related to object o1.
+   * @param[in]  o1    Name of object 1.
+   * @return     Vector of proximity objects.
+   */
+    virtual std::vector<CollisionProxy> getCollisionDistance(const std::string& o1) { throw_pretty("Not implemented!"); }
     /**
        * @brief      Gets the collision world links.
        * @return     The collision world links.

diff --git a/exotica/include/exotica/KinematicTree.h b/exotica/include/exotica/KinematicTree.h
@@ -60,16 +60,6 @@ enum BASE_TYPE
     PLANAR = 20
 };
 
-/**
-   * \brief Defines the two types of supported joints
-   */
-enum JointType
-{
-    JNT_UNUSED = 0,     //!< Undefined (will not be used)
-    JNT_PRISMATIC = 1,  //!< Prismatic joint
-    JNT_ROTARY = 2      //!< Rotary Joint
-};
-
 enum KinematicRequestFlags
 {
     KIN_FK = 0,

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
@@ -267,14 +267,16 @@ 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());
 }
 
 void Scene::updateWorld(const moveit_msgs::PlanningSceneWorldConstPtr& world)
 {
+    ps_->processPlanningSceneWorldMsg(*world);
     updateSceneFrames();
     collision_scene_->updateCollisionObjects(kinematica_.getCollisionTreeMap());
 }

diff --git a/exotica_python/src/Exotica.cpp b/exotica_python/src/Exotica.cpp
@@ -599,19 +599,26 @@ 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("loadScene", &Scene::loadScene);
-    scene.def("loadSceneFile", &Scene::loadSceneFile);
+    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);
     scene.def("cleanScene", &Scene::cleanScene);
     scene.def("isStateValid", [](Scene* instance, bool self, double safe_distance) { return instance->getCollisionScene()->isStateValid(self, safe_distance); }, py::arg("self") = true, py::arg("SafeDistance") = 0.0);
     scene.def("isCollisionFree", [](Scene* instance, const std::string& o1, const std::string& o2, double safe_distance) { return instance->getCollisionScene()->isCollisionFree(o1, o2, safe_distance); }, py::arg("Object1"), py::arg("Object2"), py::arg("SafeDistance") = 0.0);
     scene.def("getCollisionDistance", [](Scene* instance, bool self) { return instance->getCollisionScene()->getCollisionDistance(self); }, py::arg("self") = true);
     scene.def("getCollisionDistance", [](Scene* instance, const std::string& o1, const std::string& o2) { return instance->getCollisionScene()->getCollisionDistance(o1, o2); }, py::arg("Object1"), py::arg("Object2"));
-    scene.def("updateWorld", &Scene::updateWorld);
+    scene.def("getCollisionDistance",
+              [](Scene* instance, const std::string& o1) {
+                  return instance->getCollisionScene()->getCollisionDistance(o1);
+              },
+              py::arg("Object1"));
+    scene.def("updateWorld",
+              [](Scene* instance, moveit_msgs::PlanningSceneWorld& world) {
+                moveit_msgs::PlanningSceneWorldConstPtr myPtr(
+                    new moveit_msgs::PlanningSceneWorld(world));
+                instance->updateWorld(myPtr);
+              });
     scene.def("getCollisionRobotLinks", [](Scene* instance) { return instance->getCollisionScene()->getCollisionRobotLinks(); });
     scene.def("getCollisionWorldLinks", [](Scene* instance) { return instance->getCollisionScene()->getCollisionWorldLinks(); });
     scene.def("getRootFrameName", &Scene::getRootFrameName);