Add EnableObject API

mujin · Feb 8, 2024 · fc29543 · fc29543
1 parent cf8aeca
commit fc29543
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 7 deletions.
diff --git a/include/mujincontrollerclient/binpickingtask.h b/include/mujincontrollerclient/binpickingtask.h
@@ -539,6 +539,19 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource
     /// \param timeout timeout of communication
     virtual void Release(const std::string& targetname, const std::string& robotname = "", const std::string& toolname = "", const double timeout = 1);
 
+    /// \brief enables object
+    /// \param objectName name of the target to enable
+    /// \param state whether to enable
+    /// \param timeout timeout of communication
+    virtual void EnableObject(const std::string& objectName, bool state, const double timeout = 1);
+
+    /// \brief enables object link
+    /// \param objectName name of the target to enable
+    /// \param linkName link name of the target to enable
+    /// \param state whether to enable
+    /// \param timeout timeout of communication
+    virtual void EnableLink(const std::string& objectName, const std::string& linkName, bool state, const double timeout = 1);
+
     /// \brief calls planning GetRobotBridgeIOVariableString and returns the contents of the signal in a string with correct endianness
     virtual void GetRobotBridgeIOVariableString(const std::vector<std::string>& ionames, std::vector<std::string>& iovalues, const double timeout=10);
 

diff --git a/src/binpickingtask.cpp b/src/binpickingtask.cpp
@@ -1531,12 +1531,12 @@ void BinPickingTaskResource::Grab(const std::string& targetname, const std::stri
 {
     SetMapTaskParameters(_ss, _mapTaskParameters);
     _ss << GetJsonString("command", "Grab") << ", ";
-    _ss << GetJsonString("targetname", targetname) << ", ";
+    _ss << ", " << GetJsonString("targetname", targetname);
     if (!robotname.empty()) {
-        _ss << GetJsonString("robotname", robotname) << ", ";
+        _ss << ", " << GetJsonString("robotname", robotname);
     }
     if (!toolname.empty()) {
-        _ss << GetJsonString("toolname", toolname) << ", ";
+        _ss << ", " << GetJsonString("toolname", toolname);
     }
     _ss << "}";
     rapidjson::Document d;
@@ -1546,19 +1546,42 @@ void BinPickingTaskResource::Grab(const std::string& targetname, const std::stri
 void BinPickingTaskResource::Release(const std::string& targetname, const std::string& robotname, const std::string& toolname, const double timeout)
 {
     SetMapTaskParameters(_ss, _mapTaskParameters);
-    _ss << GetJsonString("command", "Release") << ", ";
-    _ss << GetJsonString("targetname", targetname) << ", ";
+    _ss << GetJsonString("command", "Release");
+    _ss << ", " << GetJsonString("targetname", targetname);
     if (!robotname.empty()) {
-        _ss << GetJsonString("robotname", robotname) << ", ";
+        _ss << ", " << GetJsonString("robotname", robotname);
     }
     if (!toolname.empty()) {
-        _ss << GetJsonString("toolname", toolname) << ", ";
+        _ss << ", " << GetJsonString("toolname", toolname);
     }
     _ss << "}";
     rapidjson::Document d;
     ExecuteCommand(_ss.str(), d, timeout);
 }
 
+void BinPickingTaskResource::EnableObject(const std::string& objectName, bool state, const double timeout)
+{
+    SetMapTaskParameters(_ss, _mapTaskParameters);
+    _ss << GetJsonString("command", "EnableObject");
+    _ss << ", " << GetJsonString("objectName", objectName);
+    _ss << ", " << GetJsonString("state", state);
+    _ss << "}";
+    rapidjson::Document d;
+    ExecuteCommand(_ss.str(), d, timeout);
+}
+
+void BinPickingTaskResource::EnableLink(const std::string& objectName, const std::string& linkName, bool state, const double timeout)
+{
+    SetMapTaskParameters(_ss, _mapTaskParameters);
+    _ss << GetJsonString("command", "EnableLink");
+    _ss << ", " << GetJsonString("objectName", objectName);
+    _ss << ", " << GetJsonString("linkName", linkName);
+    _ss << ", " << GetJsonString("state", state);
+    _ss << "}";
+    rapidjson::Document d;
+    ExecuteCommand(_ss.str(), d, timeout);
+}
+
 void BinPickingTaskResource::GetRobotBridgeIOVariableString(const std::vector<std::string>& ionames, std::vector<std::string>& iovalues, const double timeout)
 {
     SetMapTaskParameters(_ss, _mapTaskParameters);