add extrageometry structure and modify related functions

include/openrave/kinbody.h

@@ -349,7 +349,6 @@ class OPENRAVE_API KinBody : public InterfaceBase
         CLA_CheckLimitsThrow = 3, ///< check the limits and throws if something went wrong
     };
 
-
     /// \brief Describes the properties of a geometric primitive.
     ///
     /// Contains everything associated with a geometry's appearance and shape
@@ -982,6 +981,33 @@ class OPENRAVE_API KinBody : public InterfaceBase
     typedef boost::shared_ptr<Geometry> GeometryPtr;
     typedef boost::shared_ptr<Geometry const> GeometryConstPtr;
 
+    /// \brief Describes the properties of an extraGeometric primitive.
+    class OPENRAVE_API ExtraGeometryInfo : public InfoBase
+    {
+public:
+        ExtraGeometryInfo(){
+        }
+
+        void Reset() override;
+        void SerializeJSON(rapidjson::Value &value, rapidjson::Document::AllocatorType& allocator, dReal fUnitScale, int options) const override;
+        void DeserializeJSON(const rapidjson::Value &value, dReal fUnitScale, int options) override;
+
+        std::vector<GeometryInfoPtr> _vgeometryinfos;
+
+        ///\brief unique id of the link
+        std::string _id;
+        /// \brief unique link name
+        std::string _name;
+
+private:
+        friend class Link;
+        friend class KinBody;
+        friend class RobtBase;
+    };
+
+    typedef boost::shared_ptr<ExtraGeometryInfo> ExtraGeometryInfoPtr;
+    typedef boost::shared_ptr<ExtraGeometryInfo const> ExtraGeometryInfoConstPtr;
+
     /// \brief Describes the properties of a link used to initialize it
     class OPENRAVE_API LinkInfo : public InfoBase
     {
@@ -1017,7 +1043,7 @@ class OPENRAVE_API KinBody : public InterfaceBase
         std::vector<GeometryInfoPtr> _vgeometryinfos;
         /// extra-purpose geometries like
         /// self -  self-collision specific geometry. By default, this type of geometry will be always set
-        std::map< std::string, std::vector<GeometryInfoPtr> > _mapExtraGeometries;
+        std::map< std::string, ExtraGeometryInfoPtr > _mapExtraGeometries;
 
         ///\brief unique id of the link
         std::string _id;
@@ -1086,6 +1112,8 @@ class OPENRAVE_API KinBody : public InterfaceBase
         /// \brief deserializes a readable from rReadable and stores it into _mReadableInterfaces[id]
         void _DeserializeReadableInterface(const std::string& id, const rapidjson::Value& rReadable, dReal fUnitScale);
 
+        void _DeserializeExtraGeometryInfo(const std::string& id, const rapidjson::Value& rExtraGeom, dReal fUnitScale, int options);
+
         friend class Link;
         friend class KinBody;
         friend class RobtBase;
@@ -1331,25 +1359,25 @@ class OPENRAVE_API KinBody : public InterfaceBase
         ///
         /// \param name The name of the geometry group. If name is empty, will initialize the default geometries.
         /// \throw If name does not exist in GetInfo()._mapExtraGeometries, then throw an exception.
-        void SetGeometriesFromGroup(const std::string& name);
+        void SetGeometriesFromGroup(const std::string& groupid);
 
         /// \brief returns a const reference to the vector of geometries for a particular group
         ///
         /// \param name The name of the geometry group.
         /// \throw openrave_exception If the group does not exist, throws an exception.
-        const std::vector<KinBody::GeometryInfoPtr>& GetGeometriesFromGroup(const std::string& name) const;
+        const std::vector<KinBody::GeometryInfoPtr>& GetGeometriesFromGroup(const std::string& groupid) const;
 
         /// \brief stores geometries for later retrieval
         ///
         /// the list is stored inside _GetInfo()._mapExtraGeometries. Note that the pointers are copied and not the data, so
         /// any be careful not to modify the geometries afterwards
-        void SetGroupGeometries(const std::string& name, const std::vector<KinBody::GeometryInfoPtr>& geometries);
+        void SetGroupGeometries(const std::string& groupid, const KinBody::ExtraGeometryInfoPtr& extraGeometry);
 
 private:
         /// \brief stores geometries for later retrieval
         ///
         /// This call is identical to SetGroupGeometries except that it does not automatically post a Prop_LinkGeometryGroup update to the body. It will be up to the caller to ensure this occurs.
-        void _SetGroupGeometriesNoPostprocess(const std::string& name, const std::vector<KinBody::GeometryInfoPtr>& geometries);
+        void _SetGroupGeometriesNoPostprocess(const std::string& name, const KinBody::ExtraGeometryInfoPtr& extraGeometry);
 
 public:
         /// \brief returns the number of geometries stored from a particular key
@@ -2601,7 +2629,9 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /// \param linkgeometries a vector containing a collection of geometry infos ptr for each links
     /// Note that the pointers are copied and not the data, so be careful not to modify the geometries afterwards
     /// This method is faster than Link::SetGeometriesFromGroup since it makes only one change callback.
-    virtual void SetLinkGroupGeometries(const std::string& name, const std::vector< std::vector<KinBody::GeometryInfoPtr> >& linkgeometries);
+    virtual void SetLinkGroupGeometries(const std::string& id, const std::vector<KinBody::ExtraGeometryInfoPtr>& linkgeometries);
+    //  DEPRECATED: now group geometry is in ExtraGeometryInfo struct type 
+    // virtual void SetLinkGroupGeometries(const std::string& name, const std::vector< std::vector<KinBody::GeometryInfoPtr> >& linkgeometries);
 
     /// \brief Unique name of the body.
     virtual const std::string& GetName() const {

python/bindings/include/openravepy/openravepy_int.h

@@ -100,6 +100,7 @@ class PyCameraIntrinsics;
 class PyLinkInfo;
 class PyJointInfo;
 class PyGeometryInfo;
+class PyExtraGeometryInfo;
 class PyManipulatorInfo;
 class PyAttachedSensorInfo;
 class PyConnectedBodyInfo;
@@ -150,6 +151,7 @@ typedef OPENRAVE_SHARED_PTR<PyCameraIntrinsics> PyCameraIntrinsicsPtr;
 typedef OPENRAVE_SHARED_PTR<PyLinkInfo> PyLinkInfoPtr;
 typedef OPENRAVE_SHARED_PTR<PyJointInfo> PyJointInfoPtr;
 typedef OPENRAVE_SHARED_PTR<PyGeometryInfo> PyGeometryInfoPtr;
+typedef OPENRAVE_SHARED_PTR<PyExtraGeometryInfo> PyExtraGeometryInfoPtr;
 typedef OPENRAVE_SHARED_PTR<PyManipulatorInfo> PyManipulatorInfoPtr;
 typedef OPENRAVE_SHARED_PTR<PyAttachedSensorInfo> PyAttachedSensorInfoPtr;
 typedef OPENRAVE_SHARED_PTR<PyConnectedBodyInfo> PyConnectedBodyInfoPtr;
@@ -888,6 +890,7 @@ OPENRAVEPY_API PyJointPtr toPyJoint(KinBody::JointPtr pjoint, PyEnvironmentBaseP
 OPENRAVEPY_API PyLinkInfoPtr toPyLinkInfo(const KinBody::LinkInfo& linkinfo);
 OPENRAVEPY_API PyJointInfoPtr toPyJointInfo(const KinBody::JointInfo& jointinfo);
 OPENRAVEPY_API PyGeometryInfoPtr toPyGeometryInfo(const KinBody::GeometryInfo& geominfo);
+OPENRAVEPY_API PyExtraGeometryInfoPtr toPyExtraGeometryInfo(const KinBody::ExtraGeometryInfo& extrageominfo);
 OPENRAVEPY_API PyManipulatorInfoPtr toPyManipulatorInfo(const RobotBase::ManipulatorInfo& manipulatorinfo);
 OPENRAVEPY_API PyAttachedSensorInfoPtr toPyAttachedSensorInfo(const RobotBase::AttachedSensorInfo& attachedSensorinfo);
 OPENRAVEPY_API PyConnectedBodyInfoPtr toPyConnectedBodyInfo(const RobotBase::ConnectedBodyInfo& connectedBodyInfo);

python/bindings/include/openravepy/openravepy_jointinfo.h

@@ -115,6 +115,30 @@ class PyGeometryInfo
 
 typedef OPENRAVE_SHARED_PTR<PyGeometryInfo> PyGeometryInfoPtr;
 
+class PyExtraGeometryInfo
+{
+public:
+    PyExtraGeometryInfo();
+    PyExtraGeometryInfo(const KinBody::ExtraGeometryInfo& info);
+    KinBody::ExtraGeometryInfoPtr GetExtraGeometryInfo();
+
+    object SerializeJSON(dReal fUnitScale=1.0, object options=py::none_());
+    void DeserializeJSON(object obj, dReal fUnitScale=1.0, object options=py::none_());
+
+    py::list _vgeometryinfos;
+
+    std::string __repr__();
+    std::string __str__();
+
+    object _id = py::none_();
+    object _name = py::none_();
+
+private:
+    void _Update(const KinBody::ExtraGeometryInfo& info);
+};
+
+typedef OPENRAVE_SHARED_PTR<PyExtraGeometryInfo> PyExtraGeometryInfoPtr;
+
 class PyLinkInfo
 {
 public:
@@ -403,13 +427,13 @@ class PyLink : public PyReadablesContainer
     void AddGeometryToGroup(object ogeometryinfo, const std::string& groupname);
 
     void RemoveGeometryByName(const std::string& geometryname, bool removeFromAllGroups);
-    void SetGeometriesFromGroup(const std::string& name);
+    void SetGeometriesFromGroup(const std::string& groupid);
 
-    object GetGeometriesFromGroup(const std::string& name);
+    object GetGeometriesFromGroup(const std::string& groupid);
 
-    void SetGroupGeometries(const std::string& name, object ogeometryinfos);
+    void SetGroupGeometries(const std::string& groupid, object oextrageometryinfo);
 
-    int GetGroupNumGeometries(const std::string& geomname);
+    int GetGroupNumGeometries(const std::string& groupid);
 
     object GetRigidlyAttachedLinks() const;