diff --git a/CHANELOG.md b/CHANELOG.md deleted file mode 100644 index 75a73a56..00000000 --- a/CHANELOG.md +++ /dev/null @@ -1,13 +0,0 @@ -# Changelog - -## 0.70.2 (2023-11-21) - -- Fix memory leak by incorrect usage of `curl_formfree`. - -## 0.70.1 (2023-08-23) - -- Add `CreateLogEntries` API support for uploading structured log entries with attachments. - -## 0.69.1 (2023-08-23) - -- Make `PickPlaceHistoryItem` json serializable. diff --git a/CMakeLists.txt b/CMakeLists.txt index 97dc745a..13a780d0 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/include/mujincontrollerclient/binpickingtask.h b/include/mujincontrollerclient/binpickingtask.h index 4eba5142..123ebb2d 100644 --- a/include/mujincontrollerclient/binpickingtask.h +++ b/include/mujincontrollerclient/binpickingtask.h @@ -139,18 +139,6 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource std::vector 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,29 +181,32 @@ 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 size2D; ///< width and length on the MVR + std::array size2D; ///< width and height on the MVR std::array 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 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 translation; ///< Translation of the 2D MVR plane (height = 0) - std::array quaternion; ///< Rotation of the 2D MVR plane (height = 0) + std::array translation_; ///< Translation of the 2D MVR plane (height = 0) + std::array 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 @@ -223,12 +214,10 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource std::array maxCandidateSize; ///< the max candidate size expecting std::array 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 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 cornerPoint; ///< The point in 3D where the corner is located. + std::array, 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 translation; ///< translation of object w.r.t. world when picked in the container + std::array quaternion; ///< quaternion of object w.r.t. world when picked in the container + std::array translationInEndEffector; ///< translation of picked object w.r.t. end effector + std::array 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 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 translation; ///< translation of end effector w.r.t. world + std::array 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 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 msensortransform; std::map msensordata; std::map > mrGeometryInfos; ///< for every object, list of all the geometry infos - std::map 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 ®istrationResultInfo, + double timeout /* second */=5.0); + // send result of RemoveObjectsFromObjectList request virtual void SendRemoveObjectsFromObjectListResult(const std::vector& 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 diff --git a/include/mujincontrollerclient/mujincontrollerclient.h b/include/mujincontrollerclient/mujincontrollerclient.h index 4ba59153..ca7e890e 100644 --- a/include/mujincontrollerclient/mujincontrollerclient.h +++ b/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 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 DebugResourcePtr; typedef boost::weak_ptr DebugResourceWeakPtr; typedef double Real; -/// \brief an attachment to a log entry -struct LogEntryAttachment -{ - std::string filename; // filename - std::vector data; // data for the attachment -}; - -typedef boost::shared_ptr LogEntryAttachmentPtr; -typedef boost::weak_ptr 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 attachments; // a list of related attachments -}; - -typedef boost::shared_ptr LogEntryPtr; -typedef boost::weak_ptr 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& 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& logEntries, std::vector& 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 >& indices, std::vector >& vertices); virtual void SetGeometryFromRawSTL(const std::vector& 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 direction; - std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - std::array 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 ToolResourcePtr; @@ -916,8 +853,8 @@ class MUJINCLIENT_API RobotResource : public ObjectResource std::string frame_origin; std::string pk; //Real direction[3]; - std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - std::array 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 { diff --git a/include/mujincontrollerclient/mujindefinitions.h b/include/mujincontrollerclient/mujindefinitions.h index 4bd9f44b..0dafedfd 100644 --- a/include/mujincontrollerclient/mujindefinitions.h +++ b/include/mujincontrollerclient/mujindefinitions.h @@ -43,16 +43,7 @@ template inline bool FuzzyEquals(const std::vector& p, const std::ve return true; } -template inline bool FuzzyEquals(const T (&p)[N], const T (&q)[N], double epsilon=1e-4) { - for (size_t i = 0; i < N; ++i) { - if (!FuzzyEquals(p[i], q[i], epsilon)) { - return false; - } - } - return true; -} - -template inline bool FuzzyEquals(const std::array& p, const std::array& q, double epsilon=1e-4) { +template inline bool FuzzyEquals(const T (&p)[N], const T (&q)[N], double epsilon=1e-3) { for (size_t i = 0; i < N; ++i) { if (!FuzzyEquals(p[i], q[i], epsilon)) { return false; @@ -74,8 +65,8 @@ struct Transform inline bool operator==(const Transform& other) const { return !operator!=(other); } - std::array quaternion; ///< quaternion [cos(ang/2), axis*sin(ang/2)] - std::array translate; ///< translation x,y,z + Real quaternion[4]; ///< quaternion [cos(ang/2), axis*sin(ang/2)] + Real translate[3]; ///< translation x,y,z }; struct AABB @@ -90,13 +81,12 @@ struct AABB inline bool operator==(const AABB& other) const { return !operator!=(other); } - std::array pos; ///< center of AABB - std::array extents; ///< half extents of AABB + Real pos[3]; ///< center of AABB + Real extents[3]; ///< half extents of AABB }; -class MUJINCLIENT_API SensorSelectionInfo : public mujinjson::JsonSerializable +struct MUJINCLIENT_API SensorSelectionInfo : public mujinjson::JsonSerializable { -public: SensorSelectionInfo() = default; SensorSelectionInfo(const std::string& sensorNameIn, const std::string& sensorLinkNameIn) : sensorName(sensorNameIn), sensorLinkName(sensorLinkNameIn) { } @@ -122,22 +112,15 @@ class MUJINCLIENT_API SensorSelectionInfo : public mujinjson::JsonSerializable /// \brief the picking history being published from the slave. Anytime the robot goes inside of the source container, its pick history will be udpated. -class MUJINCLIENT_API PickPlaceHistoryItem : public mujinjson::JsonSerializable +struct MUJINCLIENT_API PickPlaceHistoryItem { -public: - void Reset(); - - void LoadFromJson(const rapidjson::Value& rItem) override; - void SaveToJson(rapidjson::Value& rItem, rapidjson::Document::AllocatorType& alloc) const override; - std::string pickPlaceType; ///< the type of action that ocurred can be: "picked", "placed", "touched" - std::string locationName; ///< the name of the location where picking occurred for "picked", where placing occurred when "placed", and where touching occurred for "touched" - std::string containerName; ///< the name of the container where picking occurred for "picked", where placing occurred when "placed", and where touching occurred for "touched" - unsigned long long eventTimeStampUS = 0; ///< time that the event ocurred in us (from Linux epoch). For "picked" this is the chuck time, for "placed this is the unchuck time, for "touched" this is the time when the robot supposedly stopped touching/disturbing the object. + std::string locationName; ///< the name of the region where picking occurred for "picked", where placing occurred when "placed", and where touching occurred for "touched" + unsigned long long eventTimeStampUS; ///< time that the event ocurred in us (from Linux epoch). For "picked" this is the chuck time, for "placed this is the unchuck time, for "touched" this is the time when the robot supposedly stopped touching/disturbing the object. std::string object_uri; ///< the object uri Transform objectpose; ///< 7-values in world, unit is usually mm AABB localaabb; ///< AABB of object in object frame. - unsigned long long sensorTimeStampUS = 0; ///< sensor timestamp in us (from Linux epoch) of when the object was detected in the scene + unsigned long long sensorTimeStampUS; ///< sensor timestamp in us (from Linux epoch) of when the object was detected in the scene }; /// \brief Holds the state of each region coming from the planning side. @@ -165,34 +148,6 @@ inline std::ostream& operator<<(std::ostream& os, const SensorSelectionInfo& rhs return os; } -///< \brief the type of execution verification to do -enum ExecutionVerificationMode : uint8_t -{ - EVM_Never = 0, ///< never do run-time verification of source. - EVM_LastDetection = 1, ///< do verification on the last detected items, sometimes there could be a delay so NOT RECOMMENDED. - EVM_PointCloudOnChange = 2, ///< Do verification on the real-time point cloud data only when container is known to have changed its contents (ie robot went inside). When robot goes into container and leaves, that counts as a change. (RECOMMENDED) - EVM_PointCloudAlways = 3, ///< do verification on the real-time point cloud data constantly for every snapshot. This setting is important if things other than robot can change the dest container, but can slow things down and have robot be more susceptible to point cloud noise. - EVM_PointCloudOnChangeWithDuration = 4, ///< Used for objects like cylinders that can continue to move inside the container for some time before getting into a stable state. Same as pointCloudOnChange, it does verification only when container is known to have changed its contents (ie robot went inside), but it continues to take verification and snapshots until "afterChangeDuration" has expired. For example, if afterChangeDuration is 15s, then will continue capturing the point clouds up to 15s after the robot went inside. When robot goes into container and leaves, that counts as a change. - EVM_PointCloudOnChangeFirstCycleOnly = 5, ///< Only check the point cloud verification on the first execution cycle. This prevents problems if different unexpected containers coming to robot when cycle first starts. - EVM_PointCloudOnChangeAfterGrab = 6, ///< For dest containers only. Do verification on the real-time point cloud data only when container is known to have changed and after robot has grabbed the part. When robot goes into dest container and leaves, that counts as a change. Enabling this option means the robot will stop more while grabbing object. -}; - -MUJINCLIENT_API const char* GetExecutionVerificationModeString(ExecutionVerificationMode mode); - -MUJINCLIENT_API ExecutionVerificationMode GetExecutionVerificationModeFromString(const char* pModeStr, ExecutionVerificationMode defaultMode); - - -enum MinViableRegionRegistrationMode : uint8_t { - MVRRM_None = 0, ///< registration without touching - MVRRM_Lift = 1, - MVRRM_Drag = 2, - MVRRM_PerpendicularDrag = 3, -}; - -MUJINCLIENT_API const char* GetMinViableRegionRegistrationModeString(MinViableRegionRegistrationMode mode); - -MUJINCLIENT_API MinViableRegionRegistrationMode GetMinViableRegionRegistrationModeFromString(const char* pModeStr, MinViableRegionRegistrationMode defaultMode); - } // end namespace mujin #endif diff --git a/include/mujincontrollerclient/mujinjson.h b/include/mujincontrollerclient/mujinjson.h index 6c7235bf..081c4a42 100644 --- a/include/mujincontrollerclient/mujinjson.h +++ b/include/mujincontrollerclient/mujinjson.h @@ -22,14 +22,11 @@ #include #include #include -#include #include #include #include #include -#include #include -#include #include #include @@ -165,11 +162,13 @@ inline void DumpJson(const rapidjson::Value& value, std::ostream& os, const unsi } } -/// \brief this clears the allcoator! -inline void ParseJson(rapidjson::Document& d, const char* str, size_t length) -{ +inline void ParseJson(rapidjson::Document& d, const char* str, size_t length) { + // repeatedly calling Parse on the same rapidjson::Document will not release previsouly allocated memory, memory will accumulate until the object is destroyed + // we use a new temporary Document to parse, and swap content with the original one, so that memory in original Document will be released when this function ends + // see: https://github.com/Tencent/rapidjson/issues/1333 + // a newer solution that allows reuse of allocated memory is to clear the previous document first d.SetNull(); - d.GetAllocator().Clear(); // dangerous if used by other existing objects + d.GetAllocator().Clear(); d.Parse(str, length); // parse float in full precision mode if (d.HasParseError()) { const std::string substr(str, length < 200 ? length : 200); @@ -178,12 +177,10 @@ inline void ParseJson(rapidjson::Document& d, const char* str, size_t length) } } -/// \brief this clears the allcoator! inline void ParseJson(rapidjson::Document& d, const std::string& str) { ParseJson(d, str.c_str(), str.size()); } -/// \brief this clears the allcoator! inline void ParseJson(rapidjson::Document& d, std::istream& is) { rapidjson::IStreamWrapper isw(is); // see note in: void ParseJson(rapidjson::Document& d, const std::string& str) @@ -195,27 +192,6 @@ inline void ParseJson(rapidjson::Document& d, std::istream& is) { } } -inline void ParseJson(rapidjson::Value& r, rapidjson::Document::AllocatorType& alloc, std::istream& is) -{ - rapidjson::IStreamWrapper isw(is); - rapidjson::GenericReader reader(&alloc); - //ClearStackOnExit scope(*this); - - size_t kDefaultStackCapacity = 1024; - rapidjson::Document rTemp(&alloc, kDefaultStackCapacity); // needed by Parse to be a document - rTemp.ParseStream(isw); // parse float in full precision mode - if (rTemp.HasParseError()) { - throw MujinJSONException(boost::str(boost::format("Json stream is invalid (offset %u) %s")%((unsigned)rTemp.GetErrorOffset())%GetParseError_En(rTemp.GetParseError()))); - } - r.Swap(rTemp); - -// rapidjson::ParseResult parseResult = reader.template Parse(isw, rTemp); -// if( parseResult.IsError() ) { -// *stack_.template Pop(1) -// throw MujinJSONException(boost::str(boost::format("Json stream is invalid (offset %u) %s")%((unsigned)parseResult.Offset())%GetParseError_En(parseResult.Code()))); -// } -} - template MUJINCLIENT_API void ParseJsonFile(rapidjson::Document& d, const char* filename, Container& buffer); @@ -492,60 +468,17 @@ template inline void LoadJsonValue(const rapidjson::Value& v, std::map< } } else if (v.IsObject()) { t.clear(); + U value; for (rapidjson::Value::ConstMemberIterator it = v.MemberBegin(); it != v.MemberEnd(); ++it) { - // Deserialize directly into the map to avoid copying temporaries. - // Note that our key needs to be explicitly length-constructed since - // it may contain \0 bytes. - LoadJsonValue(it->value, - t[std::string(it->name.GetString(), - it->name.GetStringLength())]); + LoadJsonValue(it->value, value); + t[std::string(it->name.GetString(), it->name.GetStringLength())] = value; // string can contain null character } } else { throw MujinJSONException("Cannot convert json type " + GetJsonTypeName(v) + " to Map"); } } -template inline void LoadJsonValue(const rapidjson::Value& v, std::deque& t) { - // It doesn't make sense to construct a deque from anything other than a JSON array - if (!v.IsArray()) { - throw MujinJSONException("Cannot convert json type " + GetJsonTypeName(v) + " to deque"); - } - - // Ensure our output is a blank slate - t.clear(); - - // Preallocate to fit the incoming data. Deque has no reserve, only resize. - t.resize(v.Size()); - - // Iterate each array entry and attempt to deserialize it directly as a member type - typename std::deque::size_type emplaceIndex = 0; - for (rapidjson::Value::ConstValueIterator it = v.Begin(); it != v.End(); ++it) { - // Deserialize directly into the map to avoid copying temporaries. - LoadJsonValue(*it, t[emplaceIndex++]); - } -} - -template inline void LoadJsonValue(const rapidjson::Value& v, std::unordered_map& t) { - // It doesn't make sense to construct an unordered map from anything other - // than a full JSON object - if (!v.IsObject()) { - throw MujinJSONException("Cannot convert json type " + GetJsonTypeName(v) + " to unordered_map"); - } - - // Ensure our output is a blank slate - t.clear(); - for (rapidjson::Value::ConstMemberIterator it = v.MemberBegin(); - it != v.MemberEnd(); ++it) { - // Deserialize directly into the map to avoid copying temporaries. - // Note that our key needs to be explicitly length-constructed since it - // may contain \0 bytes. - LoadJsonValue( - it->value, - t[std::string(it->name.GetString(), it->name.GetStringLength())]); - } -} - //Save a data structure to rapidjson::Value format /*template inline void SaveJsonValue(rapidjson::Value& v, const T& t, rapidjson::Document::AllocatorType& alloc) {*/ @@ -695,26 +628,6 @@ template inline void SaveJsonValue(rapidjson::Value& v, const std::map< } } -template inline void SaveJsonValue(rapidjson::Value& v, const std::unordered_map& t, rapidjson::Document::AllocatorType& alloc) { - v.SetObject(); - for (typename std::unordered_map::const_iterator it = t.begin(); it != t.end(); ++it) { - rapidjson::Value name, value; - SaveJsonValue(name, it->first, alloc); - SaveJsonValue(value, it->second, alloc); - v.AddMember(name, value, alloc); - } -} - -template inline void SaveJsonValue(rapidjson::Value& v, const std::deque& t, rapidjson::Document::AllocatorType& alloc) { - v.SetArray(); - v.Reserve(t.size(), alloc); - for (typename std::deque::const_iterator it = t.begin(); it != t.end(); ++it) { - rapidjson::Value value; - SaveJsonValue(value, *it, alloc); - v.PushBack(value, alloc); - } -} - template inline void SaveJsonValue(rapidjson::Document& v, const T& t) { // rapidjson::Value::CopyFrom also doesn't free up memory, need to clear memory // see note in: void ParseJson(rapidjson::Document& d, const std::string& str) @@ -829,26 +742,6 @@ inline std::string GetStringJsonValueByKey(const rapidjson::Value& v, const char return GetJsonValueByKey(v, key, defaultValue); } -/// \brief default value is returned when there is no key or value is null -inline const char* GetCStringJsonValueByKey(const rapidjson::Value& v, const char* key, const char* pDefaultValue=nullptr) { - if (!v.IsObject()) { - throw MujinJSONException("Cannot load value of non-object."); - } - rapidjson::Value::ConstMemberIterator itMember = v.FindMember(key); - if (itMember != v.MemberEnd() ) { - const rapidjson::Value& child = itMember->value; - if (!child.IsNull()) { - if( child.IsString() ) { - return child.GetString(); - } - else { - throw MujinJSONException("In GetCStringJsonValueByKey, expecting a String, but got a different object type"); - } - } - } - return pDefaultValue; // not present -} - template inline T GetJsonValueByPath(const rapidjson::Value& v, const char* key) { T r; const rapidjson::Value *child = rapidjson::Pointer(key).Get(v); diff --git a/samples/mujinexecutetask.cpp b/samples/mujinexecutetask.cpp index 65d7cbaa..4a0d1359 100644 --- a/samples/mujinexecutetask.cpp +++ b/samples/mujinexecutetask.cpp @@ -29,7 +29,7 @@ int main(int argc, char ** argv) controller = CreateControllerClient(argv[1], argv[2]); } else { - BOOST_ASSERT(0); + controller = CreateControllerClient(argv[1]); } std::cout << "connected to controller v" << controller->GetVersion() << std::endl; diff --git a/samples/mujinexecutetask_fast.cpp b/samples/mujinexecutetask_fast.cpp index 62f8a4dc..03f99a1b 100644 --- a/samples/mujinexecutetask_fast.cpp +++ b/samples/mujinexecutetask_fast.cpp @@ -29,7 +29,7 @@ int main(int argc, char ** argv) controller = CreateControllerClient(argv[1], argv[2]); } else { - BOOST_ASSERT(0); + controller = CreateControllerClient(argv[1]); } std::cout << "connected to controller v" << controller->GetVersion() << std::endl; diff --git a/samples/mujinexecutetask_robodia.cpp b/samples/mujinexecutetask_robodia.cpp index e26eaee6..9aa3cfee 100644 --- a/samples/mujinexecutetask_robodia.cpp +++ b/samples/mujinexecutetask_robodia.cpp @@ -29,7 +29,7 @@ int main(int argc, char ** argv) controller = CreateControllerClient(argv[1], argv[2]); } else { - BOOST_ASSERT(0); + controller = CreateControllerClient(argv[1]); } std::cout << "connected to controller v" << controller->GetVersion() << std::endl; diff --git a/samples/mujinideal_densowave.cpp b/samples/mujinideal_densowave.cpp index 736d7136..5187b985 100644 --- a/samples/mujinideal_densowave.cpp +++ b/samples/mujinideal_densowave.cpp @@ -31,7 +31,7 @@ int main(int argc, char ** argv) controller = CreateControllerClient(argv[1], argv[2]); } else { - BOOST_ASSERT(0); + controller = CreateControllerClient(argv[1]); } std::cout << "connected to controller v" << controller->GetVersion() << std::endl; diff --git a/samples/mujinimportscene.cpp b/samples/mujinimportscene.cpp index bd99a31e..0bf9d147 100644 --- a/samples/mujinimportscene.cpp +++ b/samples/mujinimportscene.cpp @@ -27,7 +27,7 @@ int main(int argc, char ** argv) controller = CreateControllerClient(argv[1], argv[2]); } else { - BOOST_ASSERT(0); + controller = CreateControllerClient(argv[1]); } std::cout << "connected to controller v" << controller->GetVersion() << std::endl; diff --git a/samples/mujinshowresults.cpp b/samples/mujinshowresults.cpp index c4120a1f..8eef658f 100644 --- a/samples/mujinshowresults.cpp +++ b/samples/mujinshowresults.cpp @@ -29,7 +29,7 @@ int main(int argc, char ** argv) controller = CreateControllerClient(argv[1], argv[2]); } else { - BOOST_ASSERT(0); + controller = CreateControllerClient(argv[1]); } std::cout << "connected to controller v" << controller->GetVersion() << std::endl; diff --git a/src/binpickingtask.cpp b/src/binpickingtask.cpp index 7a55c127..45c8b829 100644 --- a/src/binpickingtask.cpp +++ b/src/binpickingtask.cpp @@ -23,15 +23,13 @@ #include "mujincontrollerclient/zmq.hpp" #endif -#include - #ifdef _WIN32 #include #define isnan _isnan -#else -using std::isnan; #endif +#include + #include "logging.h" #include @@ -45,190 +43,21 @@ namespace mujinclient { using namespace utils; using namespace mujinjson; -BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo::RegisterMinViableRegionInfo() : - objectWeight(0.0), - sensorTimeStampMS(0), - robotDepartStopTimestamp(0), - transferSpeedPostMult(1.0), - minCornerVisibleDist(30), - minCornerVisibleInsideDist(0), - occlusionFreeCornerMask(0), - skipAppendingToObjectSet(false), - maxPossibleSizePadding(30) -{ - translation.fill(0); - quaternion.fill(0); - liftedWorldOffset.fill(0); - maxCandidateSize.fill(0); - minCandidateSize.fill(0); -} - -BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo::MinViableRegionInfo::MinViableRegionInfo() : - cornerMask(0) -{ - size2D.fill(0); - maxPossibleSize.fill(0); - maxPossibleSizeOriginal.fill(0); -} - -BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo::RegisterMinViableRegionInfo(const BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo& rhs) -{ - *this = rhs; -} - -BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo& BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo::operator=(const RegisterMinViableRegionInfo& rhs) -{ - minViableRegion = rhs.minViableRegion; - locationName = rhs.locationName; - translation = rhs.translation; - quaternion = rhs.quaternion; - objectWeight = rhs.objectWeight; - sensorTimeStampMS = rhs.sensorTimeStampMS; - robotDepartStopTimestamp = rhs.robotDepartStopTimestamp; - liftedWorldOffset = rhs.liftedWorldOffset; - maxCandidateSize = rhs.maxCandidateSize; - minCandidateSize = rhs.minCandidateSize; - transferSpeedPostMult = rhs.transferSpeedPostMult; - - graspModelInfo.SetNull(); - graspModelInfo.GetAllocator().Clear(); - graspModelInfo.CopyFrom(rhs.graspModelInfo, graspModelInfo.GetAllocator()); - - minCornerVisibleDist = rhs.minCornerVisibleDist; - minCornerVisibleInsideDist = rhs.minCornerVisibleInsideDist; - maxCornerAngleDeviation = rhs.maxCornerAngleDeviation; - occlusionFreeCornerMask = rhs.occlusionFreeCornerMask; - registrationMode = rhs.registrationMode; - skipAppendingToObjectSet = rhs.skipAppendingToObjectSet; - maxPossibleSizePadding = rhs.maxPossibleSizePadding; - fullDofValues = rhs.fullDofValues; - connectedBodyActiveStates = rhs.connectedBodyActiveStates; - return *this; -} - -void BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo::SerializeJSON(rapidjson::Value& rInfo, rapidjson::Document::AllocatorType& alloc) const -{ - rInfo.SetObject(); - { - rapidjson::Value rMinViableRegion; - rMinViableRegion.SetObject(); - SetJsonValueByKey(rMinViableRegion, "size2D", minViableRegion.size2D, alloc); - SetJsonValueByKey(rMinViableRegion, "maxPossibleSize", minViableRegion.maxPossibleSize, alloc); - SetJsonValueByKey(rMinViableRegion, "maxPossibleSizeOriginal", minViableRegion.maxPossibleSizeOriginal, alloc); - SetJsonValueByKey(rMinViableRegion, "cornerMask", minViableRegion.cornerMask, alloc); - SetJsonValueByKey(rMinViableRegion, "cornerMaskOriginal", minViableRegion.cornerMaskOriginal, alloc); - rInfo.AddMember("minViableRegion", rMinViableRegion, alloc); - } - - SetJsonValueByKey(rInfo, "locationName", locationName, alloc); - SetJsonValueByKey(rInfo, "translation", translation, alloc); - SetJsonValueByKey(rInfo, "quaternion", quaternion, alloc); - SetJsonValueByKey(rInfo, "objectWeight", objectWeight, alloc); - SetJsonValueByKey(rInfo, "sensorTimeStampMS", sensorTimeStampMS, alloc); - SetJsonValueByKey(rInfo, "robotDepartStopTimestamp", robotDepartStopTimestamp, alloc); - - SetJsonValueByKey(rInfo, "liftedWorldOffset", liftedWorldOffset, alloc); - SetJsonValueByKey(rInfo, "minCandidateSize", minCandidateSize, alloc); - SetJsonValueByKey(rInfo, "maxCandidateSize", maxCandidateSize, alloc); - SetJsonValueByKey(rInfo, "transferSpeedPostMult", transferSpeedPostMult, alloc); - - { - rapidjson::Value rTemp; - rTemp.CopyFrom(graspModelInfo, alloc); - rInfo.AddMember("graspModelInfo", rTemp, alloc); - } - SetJsonValueByKey(rInfo, "minCornerVisibleDist", minCornerVisibleDist, alloc); - SetJsonValueByKey(rInfo, "minCornerVisibleInsideDist", minCornerVisibleInsideDist, alloc); - SetJsonValueByKey(rInfo, "maxCornerAngleDeviation", maxCornerAngleDeviation, alloc); - - SetJsonValueByKey(rInfo, "occlusionFreeCornerMask", occlusionFreeCornerMask, alloc); - SetJsonValueByKey(rInfo, "registrationMode", mujin::GetMinViableRegionRegistrationModeString(registrationMode), alloc); - - SetJsonValueByKey(rInfo, "skipAppendingToObjectSet", skipAppendingToObjectSet, alloc); - SetJsonValueByKey(rInfo, "maxPossibleSizePadding", maxPossibleSizePadding, alloc); - - SetJsonValueByKey(rInfo, "fullDofValues", fullDofValues, alloc); - SetJsonValueByKey(rInfo, "connectedBodyActiveStates", connectedBodyActiveStates, alloc); -} - -void BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo::DeserializeJSON(const rapidjson::Value& rInfo) +static void LoadAABBFromJsonValue(const rapidjson::Value& rAABB, mujin::AABB& aabb) { - minViableRegion = MinViableRegionInfo(); - if( rInfo.IsNull() ) { - return; - } - rapidjson::Value::ConstMemberIterator itMinViableRegion = rInfo.FindMember("minViableRegion"); - if( itMinViableRegion != rInfo.MemberEnd() ) { - const rapidjson::Value& rMinViableRegion = itMinViableRegion->value; - LoadJsonValueByKey(rMinViableRegion, "size2D", minViableRegion.size2D); - LoadJsonValueByKey(rMinViableRegion, "maxPossibleSize", minViableRegion.maxPossibleSize); - LoadJsonValueByKey(rMinViableRegion, "maxPossibleSizeOriginal", minViableRegion.maxPossibleSizeOriginal); - minViableRegion.cornerMask = GetJsonValueByKey(rMinViableRegion, "cornerMask", 0); - minViableRegion.cornerMaskOriginal = GetJsonValueByKey(rMinViableRegion, "cornerMaskOriginal", 0); - } - - LoadJsonValueByKey(rInfo, "locationName", locationName); - LoadJsonValueByKey(rInfo, "translation", translation); - LoadJsonValueByKey(rInfo, "quaternion", quaternion); - objectWeight = GetJsonValueByKey(rInfo, "objectWeight", 0); - sensorTimeStampMS = GetJsonValueByKey(rInfo, "sensorTimeStampMS", 0); - robotDepartStopTimestamp = GetJsonValueByKey(rInfo, "robotDepartStopTimestamp", 0); - - LoadJsonValueByKey(rInfo, "liftedWorldOffset", liftedWorldOffset); - LoadJsonValueByKey(rInfo, "minCandidateSize", minCandidateSize); - LoadJsonValueByKey(rInfo, "maxCandidateSize", maxCandidateSize); - transferSpeedPostMult = GetJsonValueByKey(rInfo, "transferSpeedPostMult", 1.0); - - { - graspModelInfo.SetNull(); - graspModelInfo.GetAllocator().Clear(); - rapidjson::Value::ConstMemberIterator itGraspModelInfo = rInfo.FindMember("graspModelInfo"); - if( itGraspModelInfo != rInfo.MemberEnd() ) { - graspModelInfo.CopyFrom(itGraspModelInfo->value, graspModelInfo.GetAllocator()); - } - } - minCornerVisibleDist = GetJsonValueByKey(rInfo, "minCornerVisibleDist", 30); - minCornerVisibleInsideDist = GetJsonValueByKey(rInfo, "minCornerVisibleInsideDist", 0); - maxCornerAngleDeviation = GetJsonValueByKey(rInfo, "maxCornerAngleDeviation", 0); - - occlusionFreeCornerMask = GetJsonValueByKey(rInfo, "occlusionFreeCornerMask", 0); - registrationMode = mujin::GetMinViableRegionRegistrationModeFromString(mujinjson::GetStringJsonValueByKey(rInfo, "registrationMode").c_str(), mujin::MVRRM_None); - - skipAppendingToObjectSet = GetJsonValueByKey(rInfo, "skipAppendingToObjectSet", false); - maxPossibleSizePadding = GetJsonValueByKey(rInfo, "maxPossibleSizePadding", 30); - - LoadJsonValueByKey(rInfo, "fullDofValues", fullDofValues); - LoadJsonValueByKey(rInfo, "connectedBodyActiveStates", connectedBodyActiveStates); -} - -void BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo::ConvertLengthUnitScale(double fUnitScale) -{ - if( fUnitScale == 1 ) { - return; - } - minViableRegion.size2D[0] *= fUnitScale; - minViableRegion.size2D[1] *= fUnitScale; - minViableRegion.maxPossibleSize[0] *= fUnitScale; - minViableRegion.maxPossibleSize[1] *= fUnitScale; - minViableRegion.maxPossibleSize[2] *= fUnitScale; - minViableRegion.maxPossibleSizeOriginal[0] *= fUnitScale; - minViableRegion.maxPossibleSizeOriginal[1] *= fUnitScale; - minViableRegion.maxPossibleSizeOriginal[2] *= fUnitScale; - translation[0] *= fUnitScale; - translation[1] *= fUnitScale; - translation[2] *= fUnitScale; - liftedWorldOffset[0] *= fUnitScale; - liftedWorldOffset[1] *= fUnitScale; - liftedWorldOffset[2] *= fUnitScale; - maxCandidateSize[0] *= fUnitScale; - maxCandidateSize[1] *= fUnitScale; - maxCandidateSize[2] *= fUnitScale; - minCandidateSize[0] *= fUnitScale; - minCandidateSize[1] *= fUnitScale; - minCandidateSize[2] *= fUnitScale; - minCornerVisibleDist *= fUnitScale; - minCornerVisibleInsideDist *= fUnitScale; - maxPossibleSizePadding *= fUnitScale; + BOOST_ASSERT(rAABB.IsObject()); + BOOST_ASSERT(rAABB.HasMember("pos")); + BOOST_ASSERT(rAABB.HasMember("extents")); + const rapidjson::Value& rPos = rAABB["pos"]; + BOOST_ASSERT(rPos.IsArray()); + mujinjson::LoadJsonValue(rPos[0], aabb.pos[0]); + mujinjson::LoadJsonValue(rPos[1], aabb.pos[1]); + mujinjson::LoadJsonValue(rPos[2], aabb.pos[2]); + const rapidjson::Value& rExtents = rAABB["extents"]; + BOOST_ASSERT(rExtents.IsArray()); + mujinjson::LoadJsonValue(rExtents[0], aabb.extents[0]); + mujinjson::LoadJsonValue(rExtents[1], aabb.extents[1]); + mujinjson::LoadJsonValue(rExtents[2], aabb.extents[2]); } BinPickingResultResource::BinPickingResultResource(ControllerClientPtr controller, const std::string& pk) : PlanningResultResource(controller,"binpickingresult", pk) @@ -465,11 +294,11 @@ std::string utils::GetJsonString(const BinPickingTaskResource::DetectedObject& o { std::stringstream ss; ss << std::setprecision(std::numeric_limits::digits10+1); - //"{\"name\": \"obj\",\"translation\":[100,200,300],\"quaternion\":[1,0,0,0],\"confidence\":0.5}" + //"{\"name\": \"obj\",\"translation_\":[100,200,300],\"quat_\":[1,0,0,0],\"confidence\":0.5}" ss << "{"; ss << GetJsonString("name") << ": " << GetJsonString(obj.name) << ", "; ss << GetJsonString("object_uri") << ": " << GetJsonString(obj.object_uri) << ", "; - ss << GetJsonString("translation") << ": ["; + ss << GetJsonString("translation_") << ": ["; for (unsigned int i=0; i<3; i++) { ss << obj.transform.translate[i]; if (i!=3-1) { @@ -477,7 +306,7 @@ std::string utils::GetJsonString(const BinPickingTaskResource::DetectedObject& o } } ss << "], "; - ss << GetJsonString("quaternion") << ": ["; + ss << GetJsonString("quat_") << ": ["; for (unsigned int i=0; i<4; i++) { ss << obj.transform.quaternion[i]; if (i!=4-1) { @@ -661,7 +490,6 @@ void BinPickingTaskResource::ResultGetInstObjectAndSensorInfo::Parse(const rapid } LoadJsonValueByKey(it->value, "uri", muri[objname]); - LoadJsonValueByKey(it->value, "objectType", mObjectType[objname]); } const rapidjson::Value& sensors = output["sensors"]; @@ -729,15 +557,6 @@ void BinPickingTaskResource::ResultGetBinpickingState::Parse(const rapidjson::Va BOOST_ASSERT(pt.IsObject() && pt.HasMember("output")); const rapidjson::Value& v = pt["output"]; - { - rUnitInfo.SetNull(); - rUnitInfo.GetAllocator().Clear(); - const rapidjson::Value::ConstMemberIterator itUnitInfo = v.FindMember("unitInfo"); - if( itUnitInfo != v.MemberEnd() ) { - rUnitInfo.CopyFrom(itUnitInfo->value, rUnitInfo.GetAllocator()); - } - } - statusPickPlace = GetJsonValueByKey(v, "statusPickPlace", "unknown"); statusDescPickPlace = GetJsonValueByKey(v, "statusDescPickPlace", "unknown"); cycleIndex = GetJsonValueByKey(v, "statusPickPlaceCycleIndex", ""); @@ -770,14 +589,66 @@ void BinPickingTaskResource::ResultGetBinpickingState::Parse(const rapidjson::Va numLeftInSupply = GetJsonValueByPath(v, "/orderstate/numLeftInSupply", -1); placedInDest = GetJsonValueByPath(v, "/orderstate/placedInDest", -1); + registerMinViableRegionInfo.locationName = GetJsonValueByPath(v, "/registerMinViableRegionInfo/locationName", std::string()); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/translation_", registerMinViableRegionInfo.translation_); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/quat_", registerMinViableRegionInfo.quat_); + registerMinViableRegionInfo.objectWeight = GetJsonValueByPath(v, "/registerMinViableRegionInfo/objectWeight", 0); + registerMinViableRegionInfo.sensorTimeStampMS = GetJsonValueByPath(v, "/registerMinViableRegionInfo/sensorTimeStampMS", 0); + registerMinViableRegionInfo.robotDepartStopTimestamp = GetJsonValueByPath(v, "/registerMinViableRegionInfo/robotDepartStopTimestamp", 0); + registerMinViableRegionInfo.transferSpeedPostMult = GetJsonValueByPath(v, "/registerMinViableRegionInfo/transferSpeedPostMult", 1.0); { - registerMinViableRegionInfo = RegisterMinViableRegionInfo(); - const rapidjson::Value::ConstMemberIterator itRegisterMinViableRegionInfo = v.FindMember("registerMinViableRegionInfo"); - if( itRegisterMinViableRegionInfo != v.MemberEnd() ) { - registerMinViableRegionInfo.DeserializeJSON(itRegisterMinViableRegionInfo->value); + registerMinViableRegionInfo.graspModelInfo.SetNull(); + registerMinViableRegionInfo.graspModelInfo.GetAllocator().Clear(); + const rapidjson::Value* graspModelInfoJson = rapidjson::Pointer("/registerMinViableRegionInfo/graspModelInfo").Get(v); + if( !!graspModelInfoJson && graspModelInfoJson->IsObject() ) { + registerMinViableRegionInfo.graspModelInfo.CopyFrom(*graspModelInfoJson, registerMinViableRegionInfo.graspModelInfo.GetAllocator()); + } + } + registerMinViableRegionInfo.minCornerVisibleDist = GetJsonValueByPath(v, "/registerMinViableRegionInfo/minCornerVisibleDist", 30); + registerMinViableRegionInfo.minCornerVisibleInsideDist = GetJsonValueByPath(v, "/registerMinViableRegionInfo/minCornerVisibleInsideDist", 0); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/minViableRegion/size2D", registerMinViableRegionInfo.minViableRegion.size2D); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/minViableRegion/maxPossibleSize", registerMinViableRegionInfo.minViableRegion.maxPossibleSize); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/minViableRegion/maxPossibleSizeOriginal", registerMinViableRegionInfo.minViableRegion.maxPossibleSizeOriginal, registerMinViableRegionInfo.minViableRegion.maxPossibleSize); + registerMinViableRegionInfo.minViableRegion.cornerMask = GetJsonValueByPath(v, "/registerMinViableRegionInfo/minViableRegion/cornerMask", 0); + registerMinViableRegionInfo.minViableRegion.cornerMaskOriginal = GetJsonValueByPath(v, "/registerMinViableRegionInfo/minViableRegion/cornerMaskOriginal", 0); + registerMinViableRegionInfo.occlusionFreeCornerMask = GetJsonValueByPath(v, "/registerMinViableRegionInfo/occlusionFreeCornerMask", 0); + registerMinViableRegionInfo.maxPossibleSizePadding = GetJsonValueByPath(v, "/registerMinViableRegionInfo/maxPossibleSizePadding", 30); + registerMinViableRegionInfo.skipAppendingToObjectSet = GetJsonValueByPath(v, "/registerMinViableRegionInfo/skipAppendingToObjectSet", false); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/liftedWorldOffset", registerMinViableRegionInfo.liftedWorldOffset); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/minCandidateSize", registerMinViableRegionInfo.minCandidateSize); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/maxCandidateSize", registerMinViableRegionInfo.maxCandidateSize); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/fullDofValues", registerMinViableRegionInfo.fullDofValues); + LoadJsonValueByPath(v, "/registerMinViableRegionInfo/connectedBodyActiveStates", registerMinViableRegionInfo.connectedBodyActiveStates); + + LoadJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/translation", runtimeRegistrationInfo.objectInfo.translation); + LoadJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/quaternion", runtimeRegistrationInfo.objectInfo.quaternion); + LoadJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/translationInEndEffector", runtimeRegistrationInfo.objectInfo.translationInEndEffector); + LoadJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/quaternionInEndEffector", runtimeRegistrationInfo.objectInfo.quaternionInEndEffector); + runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D.clear(); + const rapidjson::Value *pChild = rapidjson::Pointer("/runtimeRegistrationInfo/objectInfo/detectionPointCloudCorners3D").Get(v); + if (!!pChild && !pChild->IsNull()) { + const rapidjson::Value& child = *pChild; + runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D.resize(child.Size()); + for(int idpcc = 0; idpcc < (int)runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D.size(); ++idpcc) { + const rapidjson::Value& result = child[idpcc]; + runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D[idpcc].cornerPoint = GetJsonValueByKey >(result, "cornerPoint"); + runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D[idpcc].cornerAxes = GetJsonValueByKey, 2> >(result, "cornerAxes"); } } + runtimeRegistrationInfo.objectInfo.unit = GetJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/unit", "mm"); + runtimeRegistrationInfo.objectInfo.pickLocationName = GetJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/pickLocationName", ""); + runtimeRegistrationInfo.objectInfo.registrationLocationName = GetJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/registrationLocationName", ""); + runtimeRegistrationInfo.objectInfo.objectWeight = GetJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/objectWeight", 0.0); + runtimeRegistrationInfo.objectInfo.sensorTimeStampMS = GetJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/sensorTimeStampMS", 0); + runtimeRegistrationInfo.objectInfo.updateTimeStampMS = GetJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/updateTimeStampMS", 0); + runtimeRegistrationInfo.endEffectorPoseInfo.poseId = GetJsonValueByPath(v, "/runtimeRegistrationInfo/endEffectorPoseInfo/poseId", -1); + LoadJsonValueByPath(v, "/runtimeRegistrationInfo/endEffectorPoseInfo/translation", runtimeRegistrationInfo.endEffectorPoseInfo.translation); + LoadJsonValueByPath(v, "/runtimeRegistrationInfo/endEffectorPoseInfo/quaternion", runtimeRegistrationInfo.endEffectorPoseInfo.quaternion); + runtimeRegistrationInfo.endEffectorPoseInfo.unit = GetJsonValueByPath(v, "/runtimeRegistrationInfo/endEffectorPoseInfo/unit", "mm"); + runtimeRegistrationInfo.endEffectorPoseInfo.updateTimeStampMS = GetJsonValueByPath(v, "/runtimeRegistrationInfo/endEffectorPoseInfo/updateTimeStampMS", 0); + runtimeRegistrationInfo.endEffectorPoseInfo.registrationLocationArrivalTimeStampMS = GetJsonValueByPath(v, "/runtimeRegistrationInfo/endEffectorPoseInfo/registrationLocationArrivalTimeStampMS", 0); + removeObjectFromObjectListInfos.clear(); if( v.HasMember("removeObjectsFromObjectList") && v["removeObjectsFromObjectList"].IsArray()) { const rapidjson::Value& rRemoveObjectFromObjectList = v["removeObjectsFromObjectList"]; @@ -792,7 +663,7 @@ void BinPickingTaskResource::ResultGetBinpickingState::Parse(const rapidjson::Va triggerDetectionCaptureInfo.timestamp = GetJsonValueByPath(v, "/triggerDetectionCaptureInfo/timestamp", 0); triggerDetectionCaptureInfo.triggerType = GetJsonValueByPath(v, "/triggerDetectionCaptureInfo/triggerType", ""); triggerDetectionCaptureInfo.locationName = GetJsonValueByPath(v, "/triggerDetectionCaptureInfo/locationName", ""); - triggerDetectionCaptureInfo.targetUpdateNamePrefix = GetJsonValueByPath(v, "/triggerDetectionCaptureInfo/targetUpdateNamePrefix", ""); + triggerDetectionCaptureInfo.targetupdatename = GetJsonValueByPath(v, "/triggerDetectionCaptureInfo/targetupdatename", ""); activeLocationTrackingInfos.clear(); if( v.HasMember("activeLocationTrackingInfos") && v["activeLocationTrackingInfos"].IsArray()) { @@ -823,7 +694,94 @@ void BinPickingTaskResource::ResultGetBinpickingState::Parse(const rapidjson::Va } pickPlaceHistoryItems.clear(); - mujinjson::LoadJsonValueByKey(v, "pickPlaceHistoryItems", pickPlaceHistoryItems); + if( v.HasMember("pickPlaceHistoryItems") && v["pickPlaceHistoryItems"].IsArray() ) { + pickPlaceHistoryItems.resize(v["pickPlaceHistoryItems"].Size()); + for(int iitem = 0; iitem < (int)pickPlaceHistoryItems.size(); ++iitem) { + const rapidjson::Value& rItem = v["pickPlaceHistoryItems"][iitem]; + pickPlaceHistoryItems[iitem].pickPlaceType = GetJsonValueByKey(rItem, "pickPlaceType", std::string()); + pickPlaceHistoryItems[iitem].locationName = GetJsonValueByKey(rItem, "locationName", std::string()); + pickPlaceHistoryItems[iitem].eventTimeStampUS = GetJsonValueByKey(rItem, "eventTimeStampUS", 0); + pickPlaceHistoryItems[iitem].object_uri = GetJsonValueByKey(rItem, "object_uri", std::string()); + + pickPlaceHistoryItems[iitem].objectpose = Transform(); + const rapidjson::Value::ConstMemberIterator itPose = rItem.FindMember("objectpose"); + if( itPose != rItem.MemberEnd() ) { + const rapidjson::Value& rObjectPose = itPose->value;; + if( rObjectPose.IsArray() && rObjectPose.Size() == 7 ) { + LoadJsonValue(rObjectPose[0], pickPlaceHistoryItems[iitem].objectpose.quaternion[0]); + LoadJsonValue(rObjectPose[1], pickPlaceHistoryItems[iitem].objectpose.quaternion[1]); + LoadJsonValue(rObjectPose[2], pickPlaceHistoryItems[iitem].objectpose.quaternion[2]); + LoadJsonValue(rObjectPose[3], pickPlaceHistoryItems[iitem].objectpose.quaternion[3]); + LoadJsonValue(rObjectPose[4], pickPlaceHistoryItems[iitem].objectpose.translate[0]); + LoadJsonValue(rObjectPose[5], pickPlaceHistoryItems[iitem].objectpose.translate[1]); + LoadJsonValue(rObjectPose[6], pickPlaceHistoryItems[iitem].objectpose.translate[2]); + } + } + + pickPlaceHistoryItems[iitem].localaabb = mujin::AABB(); + const rapidjson::Value::ConstMemberIterator itLocalAABB = rItem.FindMember("localaabb"); + if( itLocalAABB != rItem.MemberEnd() ) { + const rapidjson::Value& rLocalAABB = itLocalAABB->value; + LoadAABBFromJsonValue(rLocalAABB, pickPlaceHistoryItems[iitem].localaabb); + } + + pickPlaceHistoryItems[iitem].sensorTimeStampUS = GetJsonValueByKey(rItem, "sensorTimeStampUS", 0); + } + } +} + +BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo::RegisterMinViableRegionInfo() : + objectWeight(0.0), + sensorTimeStampMS(0), + robotDepartStopTimestamp(0), + transferSpeedPostMult(1.0), + minCornerVisibleDist(30), + minCornerVisibleInsideDist(0), + occlusionFreeCornerMask(0), + skipAppendingToObjectSet(false), + maxPossibleSizePadding(30) +{ + translation_.fill(0); + quat_.fill(0); + liftedWorldOffset.fill(0); + maxCandidateSize.fill(0); + minCandidateSize.fill(0); +} + +BinPickingTaskResource::ResultGetBinpickingState::RegisterMinViableRegionInfo::MinViableRegionInfo::MinViableRegionInfo() : + cornerMask(0) +{ + size2D.fill(0); + maxPossibleSize.fill(0); + maxPossibleSizeOriginal.fill(0); +} + +BinPickingTaskResource::ResultGetBinpickingState::RuntimeRegistrationInfo::RuntimeRegistrationInfo() +{ +} + +BinPickingTaskResource::ResultGetBinpickingState::RuntimeRegistrationInfo::ObjectInfo::ObjectInfo() : + translation({0, 0, 0}), + quaternion({1, 0, 0, 0}), + translationInEndEffector({0, 0, 0}), + quaternionInEndEffector({1, 0, 0, 0}), + unit("mm"), + pickLocationName(""), + registrationLocationName(""), + objectWeight(0.0), + sensorTimeStampMS(0), + updateTimeStampMS(0) +{ +} + +BinPickingTaskResource::ResultGetBinpickingState::RuntimeRegistrationInfo::EndEffectorPoseInfo::EndEffectorPoseInfo() : + poseId(-1), + translation({0, 0, 0}), + quaternion({1, 0, 0, 0}), + unit("mm"), + updateTimeStampMS(0), + registrationLocationArrivalTimeStampMS(0) +{ } BinPickingTaskResource::ResultGetBinpickingState::RemoveObjectFromObjectListInfo::RemoveObjectFromObjectListInfo() : @@ -1194,7 +1152,26 @@ void BinPickingTaskResource::SendMVRRegistrationResult( _ss << "}"; rapidjson::Document pt(rapidjson::kObjectType); ExecuteCommand(_ss.str(), pt, timeout); +} + +void BinPickingTaskResource::SendRuntimeRegistrationEndEffectorPoses(const rapidjson::Document &endEffectorPosesInfo, double timeout) +{ + SetMapTaskParameters(_ss, _mapTaskParameters); + _ss << GetJsonString("command", "SendRuntimeRegistrationEndEffectorPoses") << ", "; + _ss << GetJsonString("endEffectorPosesInfo", DumpJson(endEffectorPosesInfo)) << ", "; + _ss << "}"; + rapidjson::Document pt(rapidjson::kObjectType); + ExecuteCommand(_ss.str(), pt, timeout); +} +void BinPickingTaskResource::SendRuntimeRegistrationResult(const rapidjson::Document ®istrationResultInfo, double timeout) +{ + SetMapTaskParameters(_ss, _mapTaskParameters); + _ss << GetJsonString("command", "SendRuntimeRegistrationResult") << ", "; + _ss << GetJsonString("registrationResultInfo", DumpJson(registrationResultInfo)) << ", "; + _ss << "}"; + rapidjson::Document pt(rapidjson::kObjectType); + ExecuteCommand(_ss.str(), pt, timeout); } void BinPickingTaskResource::SendRemoveObjectsFromObjectListResult( @@ -1896,8 +1873,8 @@ void utils::GetSensorTransform(SceneResource& scene, const std::string& bodyname for (size_t i=0; iname == sensorname) { Transform transform; - std::copy(attachedsensors.at(i)->quaternion.begin(), attachedsensors.at(i)->quaternion.end(), transform.quaternion.begin()); - std::copy(attachedsensors.at(i)->translate.begin(), attachedsensors.at(i)->translate.end(), transform.translate.begin()); + std::copy(attachedsensors.at(i)->quaternion, attachedsensors.at(i)->quaternion+4, transform.quaternion); + std::copy(attachedsensors.at(i)->translate, attachedsensors.at(i)->translate+3, transform.translate); if (unit == "m") { //?! transform.translate[0] *= 0.001; transform.translate[1] *= 0.001; diff --git a/src/controllerclientimpl.cpp b/src/controllerclientimpl.cpp index 05e4d504..2628bd48 100644 --- a/src/controllerclientimpl.cpp +++ b/src/controllerclientimpl.cpp @@ -31,77 +31,12 @@ MUJIN_LOGGER("mujin.controllerclientcpp"); #define CURL_OPTION_SAVE_SETTER(curl, curlopt, curvalue, newvalue) CURL_OPTION_SAVER(curl, curlopt, curvalue); CURL_OPTION_SETTER(curl, curlopt, newvalue) #define CURL_INFO_GETTER(curl, curlinfo, outvalue) CHECKCURLCODE(curl_easy_getinfo(curl, curlinfo, outvalue), "curl_easy_getinfo " # curlinfo) #define CURL_PERFORM(curl) CHECKCURLCODE(curl_easy_perform(curl), "curl_easy_perform") - -struct CURLFormReleaser { - struct curl_httppost *& form; - - ~CURLFormReleaser() - { - curl_formfree(std::exchange(form, nullptr)); - } -}; +#define CURL_FORM_RELEASER(form) boost::shared_ptr __curlformreleaser ## form((void*)0, boost::bind(boost::function(curl_formfree), form)) namespace mujinclient { using namespace mujinjson; -/// \brief given a port string "80", fill ControllerClientInfo httpPort -static void _ParseClientInfoPort(const char* port, size_t length, ControllerClientInfo& clientInfo) -{ - clientInfo.httpPort = 0; - for (; length > 0; ++port, --length) { - clientInfo.httpPort = clientInfo.httpPort * 10 + (*port - '0'); - } -} - -/// \brief given a url "http[s]://[username[:password]@]hostname[:port][/path]", parse ControllerClientInfo -static void _ParseClientInfoFromURL(const char* url, ControllerClientInfo& clientInfo) -{ - clientInfo.Reset(); - const char* colonSlashSlash = strstr(url, "://"); - if (colonSlashSlash == nullptr) { - return; - } - const char* hostname = colonSlashSlash + sizeof("://") - 1; - const char* at = strstr(hostname, "@"); // not found is ok - const char* slash = strstr(hostname, "/"); // not found is ok - if (at != nullptr && (slash == nullptr || at < slash)) { - // if the at is before the slash, i.e. for the username:password - const char* usernamePassword = hostname; - hostname = at + sizeof("@") - 1; - const char* colon = strstr(usernamePassword, ":"); // not found is ok - if (colon != nullptr) { - const char* password = colon + sizeof(":") - 1; - clientInfo.username = std::string(usernamePassword, colon - usernamePassword); - clientInfo.password = std::string(password, at - password); - } else { - clientInfo.username = std::string(usernamePassword, at - usernamePassword); - } - } - const char* port = strstr(hostname, ":"); // not found is ok - if (slash == nullptr) { - if (port == nullptr) { - // no port, no slash - clientInfo.host = hostname; - } else { - // has port, no slash - const char* portStart = port + sizeof(":") - 1; - _ParseClientInfoPort(portStart, strlen(portStart), clientInfo); - clientInfo.host = std::string(hostname, port - hostname); - } - } else { - if (port != nullptr && port < slash) { - // has port before slash - const char* portStart = port + sizeof(":") - 1; - _ParseClientInfoPort(portStart, slash - portStart, clientInfo); - clientInfo.host = std::string(hostname, port - hostname); - } else { - // no port, but has slash - clientInfo.host = std::string(hostname, slash - hostname); - } - } -} - template std::wstring ParseWincapsWCNPath(const T& sourcefilename, const boost::function& ConvertToFileSystemEncoding) { @@ -159,28 +94,15 @@ std::wstring ParseWincapsWCNPath(const T& sourcefilename, const boost::function< ControllerClientImpl::ControllerClientImpl(const std::string& usernamepassword, const std::string& baseuri, const std::string& proxyserverport, const std::string& proxyuserpw, int options, double timeout) { BOOST_ASSERT( !baseuri.empty() ); - const size_t usernameindex = usernamepassword.find_first_of(':'); + size_t usernameindex = 0; + usernameindex = usernamepassword.find_first_of(':'); BOOST_ASSERT(usernameindex != std::string::npos ); - _username = usernamepassword.substr(0, usernameindex); - const std::string password = usernamepassword.substr(usernameindex + 1); + _username = usernamepassword.substr(0,usernameindex); + std::string password = usernamepassword.substr(usernameindex+1); _httpheadersjson = NULL; _httpheadersstl = NULL; _httpheadersmultipartformdata = NULL; - - size_t authorityindex = baseuri.find("//"); - if( authorityindex != std::string::npos ) { - _fulluri = baseuri.substr(0,authorityindex+2) + usernamepassword + "@" + baseuri.substr(authorityindex+2); - } - else { - // no idea what to do here.. - _fulluri = std::string("//") + usernamepassword + "@" + baseuri; - } - - _ParseClientInfoFromURL(baseuri.c_str(), _clientInfo); - _clientInfo.username = _username; - _clientInfo.password = password; - _baseuri = baseuri; // ensure trailing slash if( _baseuri[_baseuri.size()-1] != '/' ) { @@ -297,7 +219,7 @@ ControllerClientImpl::ControllerClientImpl(const std::string& usernamepassword, _charset = itcodepage->second; } #endif - MUJIN_LOG_VERBOSE("setting character set to " << _charset); + MUJIN_LOG_INFO("setting character set to " << _charset); _SetupHTTPHeadersJSON(); _SetupHTTPHeadersSTL(); _SetupHTTPHeadersMultipartFormData(); @@ -336,11 +258,6 @@ const std::string& ControllerClientImpl::GetBaseURI() const return _baseuri; } -const ControllerClientInfo& ControllerClientImpl::GetClientInfo() const -{ - return _clientInfo; -} - void ControllerClientImpl::SetCharacterEncoding(const std::string& newencoding) { boost::mutex::scoped_lock lock(_mutex); @@ -357,7 +274,6 @@ void ControllerClientImpl::SetProxy(const std::string& serverport, const std::st CURL_OPTION_SETTER(_curl, CURLOPT_UNIX_SOCKET_PATH, NULL); CURL_OPTION_SETTER(_curl, CURLOPT_PROXY, serverport.c_str()); CURL_OPTION_SETTER(_curl, CURLOPT_PROXYUSERPWD, userpw.c_str()); - _clientInfo.unixEndpoint.clear(); } void ControllerClientImpl::SetUnixEndpoint(const std::string& unixendpoint) @@ -366,7 +282,6 @@ void ControllerClientImpl::SetUnixEndpoint(const std::string& unixendpoint) CURL_OPTION_SETTER(_curl, CURLOPT_PROXY, NULL); CURL_OPTION_SETTER(_curl, CURLOPT_PROXYUSERPWD, NULL); CURL_OPTION_SETTER(_curl, CURLOPT_UNIX_SOCKET_PATH, unixendpoint.c_str()); - _clientInfo.unixEndpoint = unixendpoint; } void ControllerClientImpl::SetLanguage(const std::string& language) @@ -390,7 +305,6 @@ void ControllerClientImpl::SetAdditionalHeaders(const std::vector& { boost::mutex::scoped_lock lock(_mutex); _additionalHeaders = additionalHeaders; - _clientInfo.additionalHeaders = additionalHeaders; _SetupHTTPHeadersJSON(); _SetupHTTPHeadersSTL(); _SetupHTTPHeadersMultipartFormData(); @@ -400,7 +314,7 @@ void ControllerClientImpl::_ExecuteGraphQuery(const char* operationName, const c { rResult.SetNull(); // zero output - rapidjson::Value rResultDoc; + rapidjson::Document rResultDoc(&rAlloc); { boost::mutex::scoped_lock lock(_mutex); @@ -424,7 +338,7 @@ void ControllerClientImpl::_ExecuteGraphQuery(const char* operationName, const c } _uri = _baseuri + "api/v2/graphql"; - _CallPost(_uri, rRequestStringBuffer.GetString(), rResultDoc, rAlloc, 200, timeout); + _CallPost(_uri, rRequestStringBuffer.GetString(), rResultDoc, 200, timeout); } // parse response @@ -751,12 +665,12 @@ int ControllerClientImpl::CallGet(const std::string& relativeuri, rapidjson::Doc boost::mutex::scoped_lock lock(_mutex); _uri = _baseapiuri; _uri += relativeuri; - return _CallGet(_uri, pt, pt.GetAllocator(), expectedhttpcode, timeout); + return _CallGet(_uri, pt, expectedhttpcode, timeout); } -int ControllerClientImpl::_CallGet(const std::string& desturi, rapidjson::Value& rResponse, rapidjson::Document::AllocatorType& alloc, int expectedhttpcode, double timeout) +int ControllerClientImpl::_CallGet(const std::string& desturi, rapidjson::Document& pt, int expectedhttpcode, double timeout) { - MUJIN_LOG_DEBUG(str(boost::format("GET %s")%desturi)); + MUJIN_LOG_INFO(str(boost::format("GET %s")%desturi)); CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_TIMEOUT_MS, 0L, (long)(timeout * 1000L)); CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_HTTPHEADER, NULL, _httpheadersjson); CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_URL, NULL, desturi.c_str()); @@ -769,11 +683,11 @@ int ControllerClientImpl::_CallGet(const std::string& desturi, rapidjson::Value& long http_code = 0; CURL_INFO_GETTER(_curl, CURLINFO_RESPONSE_CODE, &http_code); if( _buffer.rdbuf()->in_avail() > 0 ) { - mujinjson::ParseJson(rResponse, alloc, _buffer); + mujinjson::ParseJson(pt, _buffer.str()); } if( expectedhttpcode != 0 && http_code != expectedhttpcode ) { - std::string error_message = GetJsonValueByKey(rResponse, "error_message"); - std::string traceback = GetJsonValueByKey(rResponse, "traceback"); + std::string error_message = GetJsonValueByKey(pt, "error_message"); + std::string traceback = GetJsonValueByKey(pt, "traceback"); throw MUJIN_EXCEPTION_FORMAT("HTTP GET to '%s' returned HTTP status %s: %s", desturi%http_code%error_message, MEC_HTTPServer); } return http_code; @@ -887,11 +801,11 @@ int ControllerClientImpl::CallPost(const std::string& relativeuri, const std::st boost::mutex::scoped_lock lock(_mutex); _uri = _baseapiuri; _uri += relativeuri; - return _CallPost(_uri, data, pt, pt.GetAllocator(), expectedhttpcode, timeout); + return _CallPost(_uri, data, pt, expectedhttpcode, timeout); } /// \brief expectedhttpcode is not 0, then will check with the returned http code and if not equal will throw an exception -int ControllerClientImpl::_CallPost(const std::string& desturi, const std::string& data, rapidjson::Value& rResult, rapidjson::Document::AllocatorType& alloc, int expectedhttpcode, double timeout) +int ControllerClientImpl::_CallPost(const std::string& desturi, const std::string& data, rapidjson::Document& pt, int expectedhttpcode, double timeout) { MUJIN_LOG_VERBOSE(str(boost::format("POST %s")%desturi)); CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_TIMEOUT_MS, 0L, (long)(timeout * 1000L)); @@ -908,13 +822,13 @@ int ControllerClientImpl::_CallPost(const std::string& desturi, const std::strin long http_code = 0; CURL_INFO_GETTER(_curl, CURLINFO_RESPONSE_CODE, &http_code); if( _buffer.rdbuf()->in_avail() > 0 ) { - ParseJson(rResult, alloc, _buffer); + ParseJson(pt, _buffer.str()); } else { - rResult.SetObject(); + pt.SetObject(); } if( expectedhttpcode != 0 && http_code != expectedhttpcode ) { - std::string error_message = GetJsonValueByKey(rResult, "error_message"); - std::string traceback = GetJsonValueByKey(rResult, "traceback"); + std::string error_message = GetJsonValueByKey(pt, "error_message"); + std::string traceback = GetJsonValueByKey(pt, "traceback"); throw MUJIN_EXCEPTION_FORMAT("HTTP POST to '%s' returned HTTP status %s: %s", desturi%http_code%error_message, MEC_HTTPServer); } return http_code; @@ -1516,7 +1430,7 @@ void ControllerClientImpl::Upgrade(std::istream& inputStream, bool autorestart, _UploadFileToControllerViaForm(inputStream, "", _baseuri+"upgrade/"+query, timeout); } else { rapidjson::Document pt(rapidjson::kObjectType); - _CallPost(_baseuri+"upgrade/"+query, "", pt, pt.GetAllocator(), 200, timeout); + _CallPost(_baseuri+"upgrade/"+query, "", pt, 200, timeout); } } @@ -1524,7 +1438,7 @@ bool ControllerClientImpl::GetUpgradeStatus(std::string& status, double &progres { boost::mutex::scoped_lock lock(_mutex); rapidjson::Document pt(rapidjson::kObjectType); - _CallGet(_baseuri+"upgrade/", pt, pt.GetAllocator(), 200, timeout); + _CallGet(_baseuri+"upgrade/", pt, 200, timeout); if(pt.IsNull()) { return false; } @@ -1542,7 +1456,7 @@ void ControllerClientImpl::Reboot(double timeout) { boost::mutex::scoped_lock lock(_mutex); rapidjson::Document pt(rapidjson::kObjectType); - _CallPost(_baseuri+"reboot/", "", pt, pt.GetAllocator(), 200, timeout); + _CallPost(_baseuri+"reboot/", "", pt, 200, timeout); } void ControllerClientImpl::DeleteAllScenes(double timeout) @@ -1595,7 +1509,7 @@ void ControllerClientImpl::ModifySceneAddReferenceObjectPK(const std::string &sc pt.AddMember("referenceobjectpk", value, pt.GetAllocator()); boost::mutex::scoped_lock lock(_mutex); - _CallPost(_baseuri + "referenceobjectpks/add/", DumpJson(pt), pt2, pt2.GetAllocator(), 200, timeout); + _CallPost(_baseuri + "referenceobjectpks/add/", DumpJson(pt), pt2, 200, timeout); } void ControllerClientImpl::ModifySceneRemoveReferenceObjectPK(const std::string &scenepk, const std::string &referenceobjectpk, double timeout) @@ -1612,7 +1526,7 @@ void ControllerClientImpl::ModifySceneRemoveReferenceObjectPK(const std::string pt.AddMember("referenceobjectpk", value, pt.GetAllocator()); boost::mutex::scoped_lock lock(_mutex); - _CallPost(_baseuri + "referenceobjectpks/remove/", DumpJson(pt), pt2, pt2.GetAllocator(), 200, timeout); + _CallPost(_baseuri + "referenceobjectpks/remove/", DumpJson(pt), pt2, 200, timeout); } void ControllerClientImpl::_UploadDirectoryToController_UTF8(const std::string& copydir_utf8, const std::string& rawuri) @@ -1918,7 +1832,7 @@ void ControllerClientImpl::_UploadFileToControllerViaForm(std::istream& inputStr // prepare form struct curl_httppost *formpost = NULL; struct curl_httppost *lastptr = NULL; - CURLFormReleaser curlFormReleaser{formpost}; + CURL_FORM_RELEASER(formpost); curl_formadd(&formpost, &lastptr, CURLFORM_COPYNAME, "files[]", CURLFORM_FILENAME, filename.empty() ? "unused" : filename.c_str(), @@ -1966,7 +1880,7 @@ void ControllerClientImpl::_UploadDataToControllerViaForm(const void* data, size // prepare form struct curl_httppost *formpost = NULL; struct curl_httppost *lastptr = NULL; - CURLFormReleaser curlFormReleaser{formpost}; + CURL_FORM_RELEASER(formpost); curl_formadd(&formpost, &lastptr, CURLFORM_PTRNAME, "files[]", CURLFORM_BUFFER, filename.empty() ? "unused" : filename.c_str(), @@ -2002,7 +1916,7 @@ void ControllerClientImpl::_DeleteFileOnController(const std::string& desturi) std::string filename = desturi.substr(_basewebdavuri.size()); rapidjson::Document pt(rapidjson::kObjectType); - _CallPost(_baseuri+"file/delete/?filename="+filename, "", pt, pt.GetAllocator(), 200, 5.0); + _CallPost(_baseuri+"file/delete/?filename="+filename, "", pt, 200, 5.0); } void ControllerClientImpl::_DeleteDirectoryOnController(const std::string& desturi) @@ -2064,7 +1978,7 @@ void ControllerClientImpl::GetDebugInfos(std::vector& debuginf void ControllerClientImpl::ListFilesInController(std::vector& fileentries, const std::string &dirname, double timeout) { rapidjson::Document pt(rapidjson::kObjectType); - _CallGet(_baseuri+"file/list/?dirname="+dirname, pt, pt.GetAllocator(), 200, timeout); + _CallGet(_baseuri+"file/list/?dirname="+dirname, pt, 200, timeout); fileentries.resize(pt.MemberCount()); size_t iobj = 0; for (rapidjson::Document::MemberIterator it = pt.MemberBegin(); it != pt.MemberEnd(); ++it) { @@ -2078,91 +1992,4 @@ void ControllerClientImpl::ListFilesInController(std::vector& fileent } } -void ControllerClientImpl::CreateLogEntries(const std::vector& logEntries, std::vector& createdLogEntryIds, double timeout) -{ - if (logEntries.empty()) { - return; - } - - boost::mutex::scoped_lock lock(_mutex); - - // prepare the cURL options - CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_TIMEOUT_MS, 0L, (long)(timeout * 1000L)); - CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_HTTPHEADER, NULL, _httpheadersmultipartformdata); - _uri = _baseuri + "api/v2/logEntry"; - CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_URL, NULL, _uri.c_str()); - _buffer.clear(); - _buffer.str(""); - CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_WRITEFUNCTION, NULL, _WriteStringStreamCallback); - CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_WRITEDATA, NULL, &_buffer); - - // prepare the form - struct curl_httppost *formpost = NULL; - struct curl_httppost *lastptr = NULL; - CURLFormReleaser curlFormReleaser{formpost}; - - rapidjson::StringBuffer& rRequestStringBuffer = _rRequestStringBufferCache; - rapidjson::Writer writer(rRequestStringBuffer); - - for (const LogEntryPtr logEntry : logEntries) { - if (!logEntry) { - continue; - } - - // add log entry content - rRequestStringBuffer.Clear(); - writer.Reset(rRequestStringBuffer); - logEntry->rEntry.Accept(writer); - std::string formName = "logEntry/" + logEntry->logType; - curl_formadd(&formpost, &lastptr, - CURLFORM_COPYNAME, formName.c_str(), - CURLFORM_COPYCONTENTS, rRequestStringBuffer.GetString(), - CURLFORM_CONTENTTYPE, "application/json", - CURLFORM_END); - - // add attachments - for (const LogEntryAttachmentPtr attachment : logEntry->attachments) { - if (!attachment) { - continue; - } - curl_formadd(&formpost, &lastptr, - CURLFORM_COPYNAME, "attachment", - CURLFORM_BUFFER, attachment->filename.c_str(), - CURLFORM_BUFFERPTR, attachment->data.data(), - CURLFORM_BUFFERLENGTH, (long)(attachment->data.size()), - CURLFORM_END); - } - } - - CURL_OPTION_SAVE_SETTER(_curl, CURLOPT_HTTPPOST, NULL, formpost); - - // perform the call - CURL_PERFORM(_curl); - - // get http status - long http_code = 0; - CURL_INFO_GETTER(_curl, CURLINFO_RESPONSE_CODE, &http_code); - if (http_code != 201) { - throw MUJIN_EXCEPTION_FORMAT("upload failed with HTTP status %d", http_code, MEC_HTTPServer); - } - - // parse the result - if (_buffer.rdbuf()->in_avail() <= 0) { - return; - } - rapidjson::Document rResultDoc; - mujinjson::ParseJson(rResultDoc, _buffer); - if (!rResultDoc.IsObject() || !rResultDoc.HasMember("logEntryIds") || !rResultDoc["logEntryIds"].IsArray()) { - throw MUJIN_EXCEPTION_FORMAT("invalid response received while uploading log entries: %s", mujinjson::DumpJson(rResultDoc), MEC_HTTPServer); - } - - // exract the log entry ids - const rapidjson::Value& logEntryIdsArray = rResultDoc["logEntryIds"]; - for (rapidjson::Value::ConstValueIterator itLogEntryId = logEntryIdsArray.Begin(); itLogEntryId != logEntryIdsArray.End(); ++itLogEntryId) { - if (itLogEntryId->IsString()) { - createdLogEntryIds.push_back(itLogEntryId->GetString()); - } - } -} - } // end namespace mujinclient diff --git a/src/controllerclientimpl.h b/src/controllerclientimpl.h index 872724be..963f0187 100644 --- a/src/controllerclientimpl.h +++ b/src/controllerclientimpl.h @@ -29,14 +29,8 @@ class ControllerClientImpl : public ControllerClient, public boost::enable_share ControllerClientImpl(const std::string& usernamepassword, const std::string& baseuri, const std::string& proxyserverport, const std::string& proxyuserpw, int options, double timeout); virtual ~ControllerClientImpl(); - const std::string& GetUserName() const override; - const std::string& GetBaseURI() const override; - const ControllerClientInfo& GetClientInfo() const override; - - std::string GetURIWithUsernamePassword() const override - { - return _fulluri; - } + virtual const std::string& GetUserName() const; + virtual const std::string& GetBaseURI() const; virtual std::string GetVersion(); virtual void SetCharacterEncoding(const std::string& newencoding); @@ -168,12 +162,6 @@ class ControllerClientImpl : public ControllerClient, public boost::enable_share void GetDebugInfos(std::vector& debuginfos, double timeout = 5); - /// \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 - void CreateLogEntries(const std::vector& logEntries, std::vector& createdLogEntryIds, double timeout = 5) override; - inline std::string GetBaseUri() const { return _baseuri; @@ -231,11 +219,11 @@ class ControllerClientImpl : public ControllerClient, public boost::enable_share //@{ /// \param desturi expects the fully resolved URI to pass to curl - int _CallGet(const std::string& desturi, rapidjson::Value& rRequest, rapidjson::Document::AllocatorType& alloc, int expectedhttpcode=200, double timeout = 5.0); + int _CallGet(const std::string& desturi, rapidjson::Document& pt, int expectedhttpcode=200, double timeout = 5.0); int _CallGet(const std::string& desturi, std::string& outputdata, int expectedhttpcode=200, double timeout = 5.0); int _CallGet(const std::string& desturi, std::vector& outputdata, int expectedhttpcode=200, double timeout = 5.0); int _CallGet(const std::string& desturi, std::ostream& outputStream, int expectedhttpcode=200, double timeout = 5.0); - int _CallPost(const std::string& desturi, const std::string& data, rapidjson::Value& rResult, rapidjson::Document::AllocatorType& alloc, int expectedhttpcode=201, double timeout = 5.0); + int _CallPost(const std::string& desturi, const std::string& data, rapidjson::Document& pt, int expectedhttpcode=201, double timeout = 5.0); /// \brief desturi is URL-encoded. Also assume _mutex is locked. virtual void _UploadFileToController_UTF8(const std::string& filename, const std::string& desturi); @@ -282,8 +270,6 @@ class ControllerClientImpl : public ControllerClient, public boost::enable_share boost::mutex _mutex; std::stringstream _buffer; std::string _baseuri, _baseapiuri, _basewebdavuri, _uri, _username; - std::string _fulluri; ///< full connection URI with username and password. http://username@password:path - ControllerClientInfo _clientInfo; curl_slist *_httpheadersjson; curl_slist *_httpheadersstl; diff --git a/src/mujincontrollerclient.cpp b/src/mujincontrollerclient.cpp index 09eb3356..c2ad77ff 100644 --- a/src/mujincontrollerclient.cpp +++ b/src/mujincontrollerclient.cpp @@ -31,86 +31,6 @@ namespace mujinclient { using namespace mujinjson; -void ControllerClientInfo::Reset() -{ - host.clear(); - httpPort = 0; - username.clear(); - password.clear(); - additionalHeaders.clear(); - unixEndpoint.clear(); -} - -void ControllerClientInfo::LoadFromJson(const rapidjson::Value& rClientInfo) -{ - mujinjson::LoadJsonValueByKey(rClientInfo, "host", host); - mujinjson::LoadJsonValueByKey(rClientInfo, "httpPort", httpPort); - mujinjson::LoadJsonValueByKey(rClientInfo, "username", username); - mujinjson::LoadJsonValueByKey(rClientInfo, "password", password); - mujinjson::LoadJsonValueByKey(rClientInfo, "additionalHeaders", additionalHeaders); - mujinjson::LoadJsonValueByKey(rClientInfo, "unixEndpoint", unixEndpoint); -} - -void ControllerClientInfo::SaveToJson(rapidjson::Value& rClientInfo, rapidjson::Document::AllocatorType& alloc) const -{ - rClientInfo.SetObject(); - if( !host.empty() ) { - mujinjson::SetJsonValueByKey(rClientInfo, "host", host, alloc); - } - if( httpPort != 0 ) { - mujinjson::SetJsonValueByKey(rClientInfo, "httpPort", httpPort, alloc); - } - if( !username.empty() ) { - mujinjson::SetJsonValueByKey(rClientInfo, "username", username, alloc); - } - if( !password.empty() ) { - mujinjson::SetJsonValueByKey(rClientInfo, "password", password, alloc); - } - if( !additionalHeaders.empty() ) { - mujinjson::SetJsonValueByKey(rClientInfo, "additionalHeaders", additionalHeaders, alloc); - } - if( !unixEndpoint.empty() ) { - mujinjson::SetJsonValueByKey(rClientInfo, "unixEndpoint", unixEndpoint, alloc); - } -} - -void ControllerClientInfo::SaveToJson(rapidjson::Document& rClientInfo) const -{ - SaveToJson(rClientInfo, rClientInfo.GetAllocator()); -} - -bool ControllerClientInfo::operator==(const ControllerClientInfo &rhs) const -{ - return host == rhs.host && - httpPort == rhs.httpPort && - username == rhs.username && - password == rhs.password && - additionalHeaders == rhs.additionalHeaders && - unixEndpoint == rhs.unixEndpoint; -} - -std::string ControllerClientInfo::GetURL(bool bIncludeNamePassword) const -{ - std::string url; - if( host.empty() ) { - return url; - } - url += "http://"; - if( bIncludeNamePassword ) { - url += username; - url += ":"; - url += password; - url += "@"; - } - - url += host; - if( httpPort != 0 ) { - url += ":"; - url += std::to_string(httpPort); - } - return url; -} - void ExtractEnvironmentStateFromPTree(const rapidjson::Value& envstatejson, EnvironmentState& envstate) { // FIXME: is this a dict or array? @@ -303,13 +223,11 @@ ObjectResource::GeometryResourcePtr ObjectResource::LinkResource::GetGeometryFro /// geomtype /// // mesh // box: half_extents - // cylinder: height, topRadius, bottomRadius + // cylinder: height, radius // sphere: radius LoadJsonValueByKey(*it,"half_extents",geometry->half_extents); LoadJsonValueByKey(*it,"height",geometry->height); LoadJsonValueByKey(*it,"radius",geometry->radius); - LoadJsonValueByKey(*it,"topRadius",geometry->topRadius); - LoadJsonValueByKey(*it,"bottomRadius",geometry->bottomRadius); return geometry; } } @@ -342,8 +260,6 @@ void ObjectResource::LinkResource::GetGeometries(std::vectorhalf_extents); LoadJsonValueByKey(*it,"height",geometry->height); LoadJsonValueByKey(*it,"radius",geometry->radius); - LoadJsonValueByKey(*it,"topRadius",geometry->topRadius); - LoadJsonValueByKey(*it,"bottomRadius",geometry->bottomRadius); geometries.push_back(geometry); } } diff --git a/src/mujindefinitions.cpp b/src/mujindefinitions.cpp index e337bfa6..27256694 100644 --- a/src/mujindefinitions.cpp +++ b/src/mujindefinitions.cpp @@ -1,10 +1,10 @@ // -*- coding: utf-8 -*- // Copyright (C) 2012-2023 MUJIN Inc. #include -#include namespace mujin { + void SensorSelectionInfo::LoadFromJson(const rapidjson::Value& rSensorSelectionInfo) { mujinjson::LoadJsonValueByKey(rSensorSelectionInfo, "sensorName", sensorName); @@ -18,163 +18,4 @@ void SensorSelectionInfo::SaveToJson(rapidjson::Value& rSensorSelectionInfo, rap mujinjson::SetJsonValueByKey(rSensorSelectionInfo, "sensorLinkName", sensorLinkName, alloc); } -void PickPlaceHistoryItem::Reset() -{ - pickPlaceType.clear(); - locationName.clear(); - containerName.clear(); - eventTimeStampUS = 0; - object_uri.clear(); - objectpose = Transform(); - localaabb = AABB(); - sensorTimeStampUS = 0; -} - -static void _LoadAABBFromJsonValue(const rapidjson::Value& rAABB, mujin::AABB& aabb) -{ - BOOST_ASSERT(rAABB.IsObject()); - BOOST_ASSERT(rAABB.HasMember("pos")); - BOOST_ASSERT(rAABB.HasMember("extents")); - const rapidjson::Value& rPos = rAABB["pos"]; - BOOST_ASSERT(rPos.IsArray()); - mujinjson::LoadJsonValue(rPos[0], aabb.pos[0]); - mujinjson::LoadJsonValue(rPos[1], aabb.pos[1]); - mujinjson::LoadJsonValue(rPos[2], aabb.pos[2]); - const rapidjson::Value& rExtents = rAABB["extents"]; - BOOST_ASSERT(rExtents.IsArray()); - mujinjson::LoadJsonValue(rExtents[0], aabb.extents[0]); - mujinjson::LoadJsonValue(rExtents[1], aabb.extents[1]); - mujinjson::LoadJsonValue(rExtents[2], aabb.extents[2]); -} - -void PickPlaceHistoryItem::LoadFromJson(const rapidjson::Value& rItem) -{ - mujinjson::LoadJsonValueByKey(rItem, "pickPlaceType", pickPlaceType); - mujinjson::LoadJsonValueByKey(rItem, "locationName", locationName); - mujinjson::LoadJsonValueByKey(rItem, "containerName", containerName); - mujinjson::LoadJsonValueByKey(rItem, "eventTimeStampUS", eventTimeStampUS); - mujinjson::LoadJsonValueByKey(rItem, "object_uri", object_uri); - - const rapidjson::Value::ConstMemberIterator itPose = rItem.FindMember("objectpose"); - if( itPose != rItem.MemberEnd() ) { - const rapidjson::Value& rObjectPose = itPose->value;; - if( rObjectPose.IsArray() && rObjectPose.Size() == 7 ) { - mujinjson::LoadJsonValue(rObjectPose[0], objectpose.quaternion[0]); - mujinjson::LoadJsonValue(rObjectPose[1], objectpose.quaternion[1]); - mujinjson::LoadJsonValue(rObjectPose[2], objectpose.quaternion[2]); - mujinjson::LoadJsonValue(rObjectPose[3], objectpose.quaternion[3]); - mujinjson::LoadJsonValue(rObjectPose[4], objectpose.translate[0]); - mujinjson::LoadJsonValue(rObjectPose[5], objectpose.translate[1]); - mujinjson::LoadJsonValue(rObjectPose[6], objectpose.translate[2]); - } - } - - const rapidjson::Value::ConstMemberIterator itLocalAABB = rItem.FindMember("localaabb"); - if( itLocalAABB != rItem.MemberEnd() ) { - const rapidjson::Value& rLocalAABB = itLocalAABB->value; - _LoadAABBFromJsonValue(rLocalAABB, localaabb); - } - - mujinjson::LoadJsonValueByKey(rItem, "sensorTimeStampUS", sensorTimeStampUS); -} - -void PickPlaceHistoryItem::SaveToJson(rapidjson::Value& rItem, rapidjson::Document::AllocatorType& alloc) const -{ - rItem.SetObject(); - mujinjson::SetJsonValueByKey(rItem, "pickPlaceType", pickPlaceType, alloc); - mujinjson::SetJsonValueByKey(rItem, "locationName", locationName, alloc); - mujinjson::SetJsonValueByKey(rItem, "containerName", containerName, alloc); - mujinjson::SetJsonValueByKey(rItem, "eventTimeStampUS", eventTimeStampUS, alloc); - mujinjson::SetJsonValueByKey(rItem, "object_uri", object_uri, alloc); - std::array posearray; - posearray[0] = objectpose.quaternion[0]; - posearray[1] = objectpose.quaternion[1]; - posearray[2] = objectpose.quaternion[2]; - posearray[3] = objectpose.quaternion[3]; - posearray[4] = objectpose.translate[0]; - posearray[5] = objectpose.translate[1]; - posearray[6] = objectpose.translate[2]; - mujinjson::SetJsonValueByKey(rItem, "objectpose", posearray, alloc); - - rapidjson::Value rLocalAABB; rLocalAABB.SetObject(); - mujinjson::SetJsonValueByKey(rLocalAABB, "pos", localaabb.pos, alloc); - mujinjson::SetJsonValueByKey(rLocalAABB, "extents", localaabb.extents, alloc); - rItem.AddMember("localaabb", rLocalAABB, alloc); - mujinjson::SetJsonValueByKey(rItem, "sensorTimeStampUS", sensorTimeStampUS, alloc); -} - -const char* GetExecutionVerificationModeString(ExecutionVerificationMode mode) -{ - switch(mode) { - case EVM_Never: return "never"; - case EVM_LastDetection: return "lastDetection"; - case EVM_PointCloudOnChange: return "pointCloudOnChange"; - case EVM_PointCloudAlways: return "pointCloudAlways"; - case EVM_PointCloudOnChangeWithDuration: return "pointCloudOnChangeWithDuration"; - case EVM_PointCloudOnChangeFirstCycleOnly: return "pointCloudOnChangeFirstCycleOnly"; - case EVM_PointCloudOnChangeAfterGrab: return "pointCloudOnChangeAfterGrab"; - } - return "(unknown)"; -} - -ExecutionVerificationMode GetExecutionVerificationModeFromString(const char* pModeStr, ExecutionVerificationMode defaultMode) -{ - if( !pModeStr || pModeStr[0] == 0 ) { - return defaultMode; - } - if( strcmp(pModeStr, "never") == 0 ) { - return EVM_Never; - } - else if( strcmp(pModeStr, "lastDetection") == 0 ) { - return EVM_LastDetection; - } - else if( strcmp(pModeStr, "pointCloudOnChange") == 0 ) { - return EVM_PointCloudOnChange; - } - else if( strcmp(pModeStr, "pointCloudAlways") == 0 ) { - return EVM_PointCloudAlways; - } - else if( strcmp(pModeStr, "pointCloudOnChangeWithDuration") == 0 ) { - return EVM_PointCloudOnChangeWithDuration; - } - else if( strcmp(pModeStr, "pointCloudOnChangeFirstCycleOnly") == 0 ) { - return EVM_PointCloudOnChangeFirstCycleOnly; - } - else if( strcmp(pModeStr, "pointCloudOnChangeAfterGrab") == 0 ) { - return EVM_PointCloudOnChangeAfterGrab; - } - throw mujinclient::MujinException(str(boost::format("Failed to parse '%s' as ExecutionVerificationMode")%pModeStr), mujinclient::MEC_InvalidArguments); -} - -MUJINCLIENT_API const char* GetMinViableRegionRegistrationModeString(MinViableRegionRegistrationMode mode) -{ - switch(mode) { - case MVRRM_None: return "None"; - case MVRRM_Lift: return "Lift"; - case MVRRM_Drag: return "Drag"; - case MVRRM_PerpendicularDrag: return "PerpendicularDrag"; - } - return "(unknown)"; -} - -MUJINCLIENT_API MinViableRegionRegistrationMode GetMinViableRegionRegistrationModeFromString(const char* pModeStr, MinViableRegionRegistrationMode defaultMode) -{ - if( pModeStr[0] == 0 ) { - return defaultMode; - } - if( strcmp(pModeStr, "None") == 0 ) { - return MVRRM_None; - } - if( strcmp(pModeStr, "Lift") == 0 ) { - return MVRRM_Lift; - } - if( strcmp(pModeStr, "Drag") == 0 ) { - return MVRRM_Drag; - } - if( strcmp(pModeStr, "PerpendicularDrag") == 0 ) { - return MVRRM_PerpendicularDrag; - } - throw mujinclient::MujinException(str(boost::format("Failed to parse '%s' as ExecutionVerificationMode")%pModeStr), mujinclient::MEC_InvalidArguments); -} - } // end namespace mujin