communication protocol for auto registration

CMakeLists.txt

@@ -18,8 +18,8 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 # Define here the needed parameters
 # make sure to change the version in docs/Makefile
 set (MUJINCLIENT_VERSION_MAJOR 0)
-set (MUJINCLIENT_VERSION_MINOR 70)
-set (MUJINCLIENT_VERSION_PATCH 2)
+set (MUJINCLIENT_VERSION_MINOR 64)
+set (MUJINCLIENT_VERSION_PATCH 0)
 set (MUJINCLIENT_VERSION ${MUJINCLIENT_VERSION_MAJOR}.${MUJINCLIENT_VERSION_MINOR}.${MUJINCLIENT_VERSION_PATCH})
 set (MUJINCLIENT_SOVERSION ${MUJINCLIENT_VERSION_MAJOR}.${MUJINCLIENT_VERSION_MINOR})
 set (CLIENT_SOVERSION ${MUJINCLIENT_VERSION_MAJOR}.${MUJINCLIENT_VERSION_MINOR})
@@ -175,11 +175,11 @@ endif()
 find_package(RapidJSON QUIET)
 if (NOT RapidJSON_FOUND)
   message(FATAL_ERROR "could not find RapidJSON")
+else()
+  include_directories(SYSTEM ${RapidJSON_INCLUDE_DIRS})
+  add_definitions("-DRAPIDJSON_ASSERT=BOOST_ASSERT")
 endif()
 
-include_directories(SYSTEM ${RapidJSON_INCLUDE_DIRS})
-add_definitions("-DRAPIDJSON_ASSERT=BOOST_ASSERT")
-
 if( Boost_FOUND )
   include_directories(${Boost_INCLUDE_DIRS})
   set(MUJINCLIENT_LINK_DIRS ${MUJINCLIENT_LINK_DIRS} ${Boost_LIBRARY_DIRS})

include/mujincontrollerclient/binpickingtask.h

@@ -139,18 +139,6 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource
         std::vector<std::string> extra;
     };
 
-    class CopyableRapidJsonDocument : public rapidjson::Document
-    {
-public:
-        // Since ResultGetBinpickingState needs to be copyable while rapidjson::Document is not, there needs to be a small wrapper
-        CopyableRapidJsonDocument& operator=(const CopyableRapidJsonDocument& other) {
-            SetNull();
-            GetAllocator().Clear();
-            CopyFrom(other, GetAllocator());
-            return *this;
-        }
-    };
-
     struct MUJINCLIENT_API ResultGetBinpickingState : public ResultBase
     {
         /// \brief holds published occlusion results of camera and container pairs
@@ -193,42 +181,43 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource
         struct RegisterMinViableRegionInfo
         {
             RegisterMinViableRegionInfo();
-            RegisterMinViableRegionInfo(const RegisterMinViableRegionInfo& rhs);
-
-            RegisterMinViableRegionInfo& operator=(const RegisterMinViableRegionInfo& rhs);
 
-            void SerializeJSON(rapidjson::Value& rInfo, rapidjson::Document::AllocatorType& allocator) const;
-            void DeserializeJSON(const rapidjson::Value& rInfo);
-
-            /// \brief scales all translational components by this value
-            void ConvertLengthUnitScale(double fUnitScale);
+            class CopyableRapidJsonDocument : public rapidjson::Document
+            {
+            public:
+                // Since ResultGetBinpickingState needs to be copyable while rapidjson::Document is not, there needs to be a small wrapper
+                CopyableRapidJsonDocument& operator=(const CopyableRapidJsonDocument& other) {
+                    SetNull();
+                    GetAllocator().Clear();
+                    CopyFrom(other, GetAllocator());
+                    return *this;
+                }
+            };
 
             struct MinViableRegionInfo
             {
                 MinViableRegionInfo();
-                std::array<double, 2> size2D; ///< width and length on the MVR
+                std::array<double, 2> size2D; ///< width and height on the MVR
                 std::array<double, 3> maxPossibleSize; ///< the max possible size of actual item
+                uint64_t cornerMask; ///< Represents the corner(s) used for corner based detection. 4 bit. -x-y = 1, +x-y = 2, -x+y=4, +x+y = 8
                 std::array<double, 3> maxPossibleSizeOriginal; ///< the maxPossibleSize that has originally been given from vision
-                uint8_t cornerMaskOriginal; ///< the cornerMask that has originally been given from vision
-                uint8_t cornerMask; ///< Represents the corner(s) used for corner based detection. 4 bit. -x-y = 1, +x-y = 2, -x+y = 4, +x+y = 8
+                uint64_t cornerMaskOriginal; ///< the cornerMask that has originally been given from vision
             } minViableRegion;
 
             std::string locationName; ///< The name of the location where the minViableRegion was triggered for
-            std::array<double, 3> translation; ///< Translation of the 2D MVR plane (height = 0)
-            std::array<double, 4> quaternion; ///< Rotation of the 2D MVR plane (height = 0)
+            std::array<double, 3> translation_; ///< Translation of the 2D MVR plane (height = 0)
+            std::array<double, 4> quat_; ///< Rotation of the 2D MVR plane (height = 0)
             double objectWeight; ///< If non-zero, use this weight fo registration. unit is kg. zero means unknown.
             uint64_t sensorTimeStampMS; ///< Same as DetectedObject's timestamp sent to planning
             double robotDepartStopTimestamp; ///< Force capture after robot stops
             std::array<double, 3> liftedWorldOffset; ///< [dx, dy, dz], mm in world frame
             std::array<double, 3> maxCandidateSize; ///< the max candidate size expecting
             std::array<double, 3> minCandidateSize; ///< the min candidate size expecting
             double transferSpeedPostMult; ///< transfer speed multiplication factor
-            rapidjson::Document graspModelInfo; ///< Parameters used for grasping model generation for the object
+            CopyableRapidJsonDocument graspModelInfo; ///< Parameters used for grasping model generation for the object
             double minCornerVisibleDist; ///< how much distance along with uncertain edge from uncertain corner robot exposes to camera
             double minCornerVisibleInsideDist; ///< how much distance inside MVR robot exposes to camera
-            double maxCornerAngleDeviation; ///< how much angle deviation around uncertain corner is considered to expose to camera
-            uint8_t occlusionFreeCornerMask; ///< mask of corners that robot exposes to camera. 4 bit. -x-y = 1, +x-y = 2, -x+y = 4, +x+y = 8
-            mujin::MinViableRegionRegistrationMode registrationMode; ///< lift, drag or perpendicular drag
+            uint64_t occlusionFreeCornerMask; ///< mask of corners that robot exposes to camera
             bool skipAppendingToObjectSet; ///<  if true, skip appending newly created registration data into an active object set
             double maxPossibleSizePadding; ///< how much to additionally expose max possible size region to vision
             std::vector<double> fullDofValues; ///< robot configuration state on capturing
@@ -238,6 +227,52 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource
             }
         } registerMinViableRegionInfo;
 
+        /**
+         * Information needed to register a new object at runtime
+         */
+        struct RuntimeRegistrationInfo
+        {
+            RuntimeRegistrationInfo();
+
+            struct DetectionPointCloudCorner3D
+            {
+                std::array<double, 3> cornerPoint; ///< The point in 3D where the corner is located.
+                std::array<std::array<double, 3>, 2> cornerAxes; ///< Two normalized vectors that describe the axes of the corner in 3D.
+            };
+
+            struct ObjectInfo
+            {
+                ObjectInfo();
+
+                /** Information about the object that was picked **/
+                std::array<double, 3> translation; ///< translation of object w.r.t. world when picked in the container
+                std::array<double, 4> quaternion; ///< quaternion of object w.r.t. world when picked in the container
+                std::array<double, 3> translationInEndEffector; ///< translation of picked object w.r.t. end effector
+                std::array<double, 4> quaternionInEndEffector; ///< quaternion of picked object w.r.t. end effector
+                std::string unit; ///< the unit of translation
+                std::string pickLocationName; ///< The name of the location where the item was
+                std::string registrationLocationName; ///< The name of the location for registering object
+                double objectWeight; ///< If non-zero, use this weight fo registration. unit is kg. zero means unknown.
+                uint64_t sensorTimeStampMS; ///< sensor timestamp of the item. If non-zero, then valid.
+                uint64_t updateTimeStampMS; ///< timestamp this request was sent. If non-zero, then valid.
+                std::vector<DetectionPointCloudCorner3D> detectionPointCloudCorners3D;  ///< a list of 3D corners in the pointcloud from which the detection was created.
+
+            } objectInfo;
+
+            struct EndEffectorPoseInfo
+            {
+                EndEffectorPoseInfo();
+
+                /** Information about the current EndEffectorPose **/
+                int poseId; ///< id to represent which pose this is among end effector poses given from vision
+                std::array<double, 3> translation; ///< translation of end effector w.r.t. world
+                std::array<double, 4> quaternion; ///< quaternion of end effector w.r.t. world
+                std::string unit; ///< the unit of translation
+                uint64_t updateTimeStampMS; ///< timestamp this request was sent. If non-zero, then valid.
+                uint64_t registrationLocationArrivalTimeStampMS; ///< timestamp the robot will arrive at registration location. If non-zero, then valid.
+            } endEffectorPoseInfo;
+        } runtimeRegistrationInfo;
+
         struct RemoveObjectFromObjectListInfo {
             RemoveObjectFromObjectListInfo();
             double timestamp; // timestamp this request was sent. If non-zero, then valid.
@@ -253,11 +288,10 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource
             double timestamp; ///< timestamp this request was sent. If non-zero, then valid.
             std::string triggerType; ///< The type of trigger this is. For now can be: "phase1Detection", "phase2Detection"
             std::string locationName; ///< The name of the location for this detection trigger.
-            std::string targetUpdateNamePrefix; ///< if not empty, use this new targetUpdateNamePrefix for the triggering, otherwise do not change the original targetUpdateNamePrefix
+            std::string targetupdatename; ///< if not empty, use this new targetupdatename for the triggering, otherwise do not change the original targetupdatename
         } triggerDetectionCaptureInfo;
 
         std::vector<mujin::PickPlaceHistoryItem> pickPlaceHistoryItems; ///< history of pick/place actions that occurred in planning. Events should be sorted in the order they happen, ie event [0] happens before event [1], meaning event[0].eventTimeStampUS is before event[1].eventTimeStampUS
-        CopyableRapidJsonDocument rUnitInfo; ///< the unitInfo struct that specifies what the units for the data in this struct are
     };
 
     struct MUJINCLIENT_API ResultIsRobotOccludingBody : public ResultBase
@@ -321,7 +355,6 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource
         std::map<mujin::SensorSelectionInfo, Transform> msensortransform;
         std::map<mujin::SensorSelectionInfo, RobotResource::AttachedSensorResource::SensorData> msensordata;
         std::map<std::string, boost::shared_ptr<rapidjson::Document> > mrGeometryInfos; ///< for every object, list of all the geometry infos
-        std::map<std::string, std::string> mObjectType;
     };
 
     struct MUJINCLIENT_API ResultHeartBeat : public ResultBase
@@ -580,6 +613,14 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource
         const rapidjson::Document &mvrResultInfo,
         double timeout /* second */=5.0);
 
+    virtual void SendRuntimeRegistrationEndEffectorPoses(
+        const rapidjson::Document &endEffectorPosesInfo,
+        double timeout /* second */=5.0);
+
+    virtual void SendRuntimeRegistrationResult(
+        const rapidjson::Document &registrationResultInfo,
+        double timeout /* second */=5.0);
+
     // send result of RemoveObjectsFromObjectList request
     virtual void SendRemoveObjectsFromObjectListResult(const std::vector<ResultGetBinpickingState::RemoveObjectFromObjectListInfo>& removeObjectFromObjectListInfos, const bool success, const double timeout /* second */=5.0);
 
@@ -604,7 +645,7 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource
     rapidjson::Document _rUserInfo;  ///< userinfo json
     rapidjson::Document _rSceneParams; ///\ parameters of the scene to run tasks on the backend zmq slave
     std::string _sceneparams_json, _userinfo_json;
-
+    
     std::string _slaverequestid; ///< to ensure the same slave is used for binpicking task
     std::string _scenepk; ///< scene pk
     std::string _callerid; ///< string identifying the caller

include/mujincontrollerclient/mujincontrollerclient.h

@@ -63,34 +63,6 @@
 
 namespace mujinclient {
 
-/// \brief connecting to a controller's webstack
-class MUJINCLIENT_API ControllerClientInfo : public mujinjson::JsonSerializable
-{
-public:
-    virtual void Reset();
-
-    void LoadFromJson(const rapidjson::Value& rClientInfo) override;
-    void SaveToJson(rapidjson::Value& rClientInfo, rapidjson::Document::AllocatorType& alloc) const override;
-    void SaveToJson(rapidjson::Document& rClientInfo) const override;
-
-    bool operator==(const ControllerClientInfo &rhs) const;
-    bool operator!=(const ControllerClientInfo &rhs) const {
-        return !operator==(rhs);
-    }
-    std::string GetURL(bool bIncludeNamePassword) const;
-
-    inline bool IsEnabled() const {
-        return !host.empty();
-    }
-
-    std::string host;
-    uint16_t httpPort = 0; ///< Post to communicate with the webstack. If 0, then use the default port
-    std::string username;
-    std::string password;
-    std::vector<std::string> additionalHeaders; ///< expect each value to be in the format of "Header-Name: header-value"
-    std::string unixEndpoint; ///< unix socket endpoint for communicating with HTTP server over unix socket
-};
-
 typedef mujin::Transform Transform;
 
 enum TaskResourceOptions
@@ -139,27 +111,6 @@ typedef boost::shared_ptr<DebugResource> DebugResourcePtr;
 typedef boost::weak_ptr<DebugResource> DebugResourceWeakPtr;
 typedef double Real;
 
-/// \brief an attachment to a log entry
-struct LogEntryAttachment
-{
-    std::string filename; // filename
-    std::vector<unsigned char> data; // data for the attachment
-};
-
-typedef boost::shared_ptr<LogEntryAttachment> LogEntryAttachmentPtr;
-typedef boost::weak_ptr<LogEntryAttachment> LogEntryAttachmentWeakPtr;
-
-/// \brief a log entry in mujin controller
-struct LogEntry
-{
-    rapidjson::Value rEntry; // log entry data in JSON format
-    std::string logType; // log type
-    std::vector<LogEntryAttachmentPtr> attachments; // a list of related attachments
-};
-
-typedef boost::shared_ptr<LogEntry> LogEntryPtr;
-typedef boost::weak_ptr<LogEntry> LogEntryWeakPtr;
-
 /// \brief status code for a job
 ///
 /// Definitions are very similar to http://ros.org/doc/api/actionlib_msgs/html/msg/GoalStatus.html
@@ -389,12 +340,6 @@ class MUJINCLIENT_API ControllerClient
     /// \brief returns the URI used to setup the connection
     virtual const std::string& GetBaseURI() const = 0;
 
-    /// \brief full connection URI with username and password. http://username@password:path
-    virtual std::string GetURIWithUsernamePassword() const = 0;
-
-    /// \brief returns the client info used to construct this client
-    virtual const ControllerClientInfo& GetClientInfo() const = 0;
-
     /// \brief If necessary, changes the proxy to communicate to the controller server. Setting proxy disables previously set unix endpoint.
     ///
     /// \param serverport Specify proxy server to use. To specify port number in this string, append :[port] to the end of the host name. The proxy string may be prefixed with [protocol]:// since any such prefix will be ignored. The proxy's port number may optionally be specified with the separate option. If not specified, will default to using port 1080 for proxies. Setting to empty string will disable the proxy.
@@ -719,12 +664,6 @@ class MUJINCLIENT_API ControllerClient
 
     /// \brief get debug infos
     virtual void GetDebugInfos(std::vector<DebugResourcePtr>& debuginfos, double timeout = 5) = 0;
-
-    /// \brief create log entries and attachments such as images, additional files, etc.
-    /// \param logEntries a vector of log entries to upload
-    /// \param createdLogEntryIds an optional vector for storing the created log entry ids
-    /// \param timeout timeout of uploading log entries in seconds
-    virtual void CreateLogEntries(const std::vector<LogEntryPtr>& logEntries, std::vector<std::string>& createdLogEntryIds, double timeout = 5) = 0;
 };
 
 class MUJINCLIENT_API WebResource
@@ -792,8 +731,6 @@ class MUJINCLIENT_API ObjectResource : public WebResource
         Real half_extents[3];
         Real height;
         Real radius;
-        Real topRadius;
-        Real bottomRadius;
 
         virtual void GetMesh(std::string& primitive, std::vector<std::vector<int> >& indices, std::vector<std::vector<Real> >& vertices);
         virtual void SetGeometryFromRawSTL(const std::vector<unsigned char>& rawstldata, const std::string& unit, double timeout = 5.0);
@@ -900,9 +837,9 @@ class MUJINCLIENT_API RobotResource : public ObjectResource
         std::string frame_origin;
         std::string frame_tip;
         std::string pk;
-        std::array<Real,3> direction;
-        std::array<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
-        std::array<Real,3> translate;
+        Real direction[3];
+        Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
+        Real translate[3];
     };
     typedef boost::shared_ptr<ToolResource> ToolResourcePtr;
 
@@ -916,8 +853,8 @@ class MUJINCLIENT_API RobotResource : public ObjectResource
         std::string frame_origin;
         std::string pk;
         //Real direction[3];
-        std::array<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
-        std::array<Real,3> translate;
+        Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
+        Real translate[3];
         std::string sensortype;
 
         struct SensorData {