Merge pull request RobotLocomotion#7496 from EricCousineau-TRI/featur…

…e/call_python_pr1 Add `call_python` client and test code.
mcx · Nov 28, 2017 · a32d733 · a32d733
2 parents a833ead + e4c7c99
commit a32d733
Show file tree

Hide file tree

Showing 12 changed files with 943 additions and 27 deletions.
diff --git a/drake/common/proto/BUILD.bazel b/drake/common/proto/BUILD.bazel
@@ -8,6 +8,7 @@ load(
 load(
     "@drake//tools/skylark:drake_proto.bzl",
     "drake_cc_proto_library",
+    "drake_py_proto_library",
 )
 load("//tools/lint:lint.bzl", "add_lint_tests")
 
@@ -20,9 +21,15 @@ drake_cc_proto_library(
     ],
 )
 
+drake_py_proto_library(
+    name = "matlab_rpc_py",
+    srcs = [
+        "matlab_rpc.proto",
+    ],
+)
+
 drake_cc_library(
     name = "call_matlab",
-    testonly = 1,
     srcs = ["call_matlab.cc"],
     hdrs = ["call_matlab.h"],
     deps = [
@@ -40,6 +47,34 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "call_python",
+    srcs = ["call_python.cc"],
+    hdrs = ["call_python.h"],
+    deps = [
+        ":call_matlab",
+        "//drake/common:copyable_unique_ptr",
+    ],
+)
+
+py_library(
+    name = "call_python_client",
+    srcs = ["call_python_client.py"],
+    imports = ["."],
+    deps = [
+        ":matlab_rpc_py",
+    ],
+)
+
+py_binary(
+    name = "call_python_client_cli",
+    srcs = ["call_python_client.py"],
+    main = "call_python_client.py",
+    deps = [
+        ":call_python_client",
+    ],
+)
+
 # === test/ ===
 
 drake_cc_googletest(
@@ -49,6 +84,30 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "call_python_test",
+    srcs = ["test/call_python_test.cc"],
+    tags = ["manual"],
+    deps = [
+        ":call_python",
+    ],
+)
+
+# TODO(eric.cousineau): Remove --no_plotting once we can open `$DISPLAY` on CI.
+sh_test(
+    name = "call_python_full_test",
+    srcs = ["test/call_python_full_test.sh"],
+    args = [
+        "--no_plotting",
+        "$(location :call_python_test)",
+        "$(location :call_python_client_cli)",
+    ],
+    data = [
+        ":call_python_client_cli",
+        ":call_python_test",
+    ],
+)
+
 drake_cc_googletest(
     name = "protobuf_test",
     data = [

diff --git a/drake/common/proto/call_matlab.cc b/drake/common/proto/call_matlab.cc
@@ -9,9 +9,6 @@
 #include <limits>
 #include <string>
 
-#include "google/protobuf/io/coded_stream.h"
-#include "google/protobuf/io/zero_copy_stream_impl.h"
-
 #include "drake/common/drake_assert.h"
 #include "drake/common/never_destroyed.h"
 
@@ -31,6 +28,7 @@ MatlabRemoteVariable::MatlabRemoteVariable()
 
 void ToMatlabArray(const MatlabRemoteVariable& var, MatlabArray* matlab_array) {
   matlab_array->set_type(MatlabArray::REMOTE_VARIABLE_REFERENCE);
+  matlab_array->set_shape_type(MatlabArray::SCALAR);
   matlab_array->set_rows(1);
   matlab_array->set_cols(1);
   int num_bytes = sizeof(int64_t);
@@ -40,26 +38,52 @@ void ToMatlabArray(const MatlabRemoteVariable& var, MatlabArray* matlab_array) {
 
 void ToMatlabArray(double var, MatlabArray* matlab_array) {
   matlab_array->set_type(MatlabArray::DOUBLE);
+  matlab_array->set_shape_type(MatlabArray::SCALAR);
   matlab_array->set_rows(1);
   matlab_array->set_cols(1);
   int num_bytes = sizeof(double);
   matlab_array->set_data(&var, num_bytes);
 }
 
-void ToMatlabArray(const Eigen::Ref<const Eigen::MatrixXd>& mat,
-                   MatlabArray* matlab_array) {
+void internal::ToMatlabArrayMatrix(
+    const Eigen::Ref<const Eigen::MatrixXd>& mat,
+    MatlabArray* matlab_array, bool is_vector) {
   matlab_array->set_type(MatlabArray::DOUBLE);
+  matlab_array->set_shape_type(
+      is_vector ? MatlabArray::VECTOR : MatlabArray::MATRIX);
   matlab_array->set_rows(mat.rows());
   matlab_array->set_cols(mat.cols());
   int num_bytes = sizeof(double) * mat.rows() * mat.cols();
   matlab_array->set_data(mat.data(), num_bytes);
 }
 
-void ToMatlabArray(
-    const Eigen::Ref<const Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>>&
-        mat,
-    MatlabArray* matlab_array) {
+void ToMatlabArray(int var, MatlabArray* matlab_array) {
+  matlab_array->set_type(MatlabArray::INT);
+  matlab_array->set_shape_type(MatlabArray::SCALAR);
+  matlab_array->set_rows(1);
+  matlab_array->set_cols(1);
+  int num_bytes = sizeof(int);
+  matlab_array->set_data(&var, num_bytes);
+}
+
+void internal::ToMatlabArrayMatrix(
+    const Eigen::Ref<const Eigen::MatrixXi>& mat,
+    MatlabArray* matlab_array, bool is_vector) {
+  matlab_array->set_type(MatlabArray::INT);
+  matlab_array->set_shape_type(
+      is_vector ? MatlabArray::VECTOR : MatlabArray::MATRIX);
+  matlab_array->set_rows(mat.rows());
+  matlab_array->set_cols(mat.cols());
+  int num_bytes = sizeof(int) * mat.rows() * mat.cols();
+  matlab_array->set_data(mat.data(), num_bytes);
+}
+
+void internal::ToMatlabArrayMatrix(
+    const Eigen::Ref<const MatrixX<bool>>& mat,
+    MatlabArray* matlab_array, bool is_vector) {
   matlab_array->set_type(MatlabArray::LOGICAL);
+  matlab_array->set_shape_type(
+      is_vector ? MatlabArray::VECTOR : MatlabArray::MATRIX);
   matlab_array->set_rows(mat.rows());
   matlab_array->set_cols(mat.cols());
   int num_bytes = sizeof(bool) * mat.rows() * mat.cols();
@@ -68,22 +92,40 @@ void ToMatlabArray(
 
 void ToMatlabArray(const std::string& str, MatlabArray* matlab_array) {
   matlab_array->set_type(MatlabArray::CHAR);
+  matlab_array->set_shape_type(MatlabArray::VECTOR);
   matlab_array->set_rows(1);
   matlab_array->set_cols(str.length());
   int num_bytes = sizeof(char) * str.length();
   matlab_array->set_data(str.data(), num_bytes);
 }
 
-void internal::PublishCallMatlab(const MatlabRPC& message) {
-  // TODO(russt): Provide option for setting the filename.
+namespace internal {
 
+std::unique_ptr<google::protobuf::io::FileOutputStream>
+CreateOutputStream(const std::string& filename) {
   // NOTE(russt): This code violates the style-guide by expecting the file to be
-  // closed properly at program termination.  Experimentally we found that the
-  // output file could be corrupt unless we included *both* the Flush() call and
-  // the SetCloseOnDelete() calls below.  Sadly, I cannot explain why.
-  static google::protobuf::io::FileOutputStream raw_output(
-      open("/tmp/matlab_rpc", O_WRONLY | O_CREAT, S_IRWXU));
-  raw_output.SetCloseOnDelete(true);
+  // closed properly at program termination (these streams will be stored as
+  // static globals).  Experimentally we found that the output file could be
+  // corrupt unless we included *both* the Flush() call and the
+  // SetCloseOnDelete() calls below.  Sadly, I cannot explain why.
+  auto raw_output =
+      std::make_unique<google::protobuf::io::FileOutputStream>(
+          open(filename.c_str(), O_WRONLY | O_CREAT, S_IRWXU));
+  raw_output->SetCloseOnDelete(true);
+  return raw_output;
+}
+
+void PublishCallMatlab(const MatlabRPC& message) {
+  // TODO(russt): Provide option for setting the filename.
+  static auto raw_output = CreateOutputStream("/tmp/matlab_rpc");
+  PublishCall(raw_output.get(), message);
+}
+
+void PublishCall(
+    google::protobuf::io::FileOutputStream* praw_output,
+    const MatlabRPC& message) {
+  DRAKE_DEMAND(praw_output);
+  auto& raw_output = *praw_output;
 
   {  // Defines the lifetime of the CodedOutputStream.
     google::protobuf::io::CodedOutputStream output(&raw_output);
@@ -107,5 +149,6 @@ void internal::PublishCallMatlab(const MatlabRPC& message) {
   raw_output.Flush();
 }
 
+}  // namespace internal
 }  // namespace common
 }  // namespace drake
diff --git a/drake/common/proto/call_matlab.h b/drake/common/proto/call_matlab.h
@@ -1,12 +1,17 @@
 #pragma once
 
+#include <memory>
 #include <string>
 #include <vector>
 
+#include "google/protobuf/io/coded_stream.h"
+#include "google/protobuf/io/zero_copy_stream_impl.h"
+
 #include "drake/common/eigen_types.h"
 #include "drake/common/proto/matlab_rpc.pb.h"
 
-/// @file Utilities for calling Matlab from C++
+/// @file
+/// @brief Utilities for calling Matlab from C++
 ///
 /// Provides a simple interface for (one-directional) RPC to a simple matlab
 /// remote client.  Methods are provided to serialize our favorite data types
@@ -31,7 +36,6 @@
 ///
 /// See call_matlab_test.cc for some simple examples.
 
-
 namespace drake {
 namespace common {
 
@@ -40,20 +44,37 @@ namespace common {
 ///  another one of these methods.
 
 class MatlabRemoteVariable;
-void ToMatlabArray(const MatlabRemoteVariable& var, MatlabArray* matlab_array);
 
-void ToMatlabArray(double scalar, MatlabArray* matlab_array);
+namespace internal {
 
-void ToMatlabArray(
+void ToMatlabArrayMatrix(
     const Eigen::Ref<const Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>>&
         mat,
-    MatlabArray* matlab_array);
+    MatlabArray* matlab_array, bool is_vector);
 
-void ToMatlabArray(const Eigen::Ref<const Eigen::MatrixXd>& mat,
-                   MatlabArray* matlab_array);
+void ToMatlabArrayMatrix(const Eigen::Ref<const Eigen::MatrixXd>& mat,
+                         MatlabArray* matlab_array, bool is_vector);
+
+void ToMatlabArrayMatrix(const Eigen::Ref<const Eigen::MatrixXi>& mat,
+                         MatlabArray* matlab_array, bool is_vector);
+
+}  // namespace internal
+
+void ToMatlabArray(const MatlabRemoteVariable& var, MatlabArray* matlab_array);
+
+void ToMatlabArray(double scalar, MatlabArray* matlab_array);
+
+void ToMatlabArray(int scalar, MatlabArray* matlab_array);
 
 void ToMatlabArray(const std::string& str, MatlabArray* matlab_array);
 
+template <typename Derived>
+void ToMatlabArray(const Eigen::MatrixBase<Derived>& mat,
+                   MatlabArray* matlab_array) {
+  const bool is_vector = (Derived::ColsAtCompileTime == 1);
+  return internal::ToMatlabArrayMatrix(mat, matlab_array, is_vector);
+}
+
 // Helper methods for variadic template call in CallMatlab.
 namespace internal {
 inline void AssembleCallMatlabMsg(MatlabRPC*) {
@@ -66,6 +87,13 @@ void AssembleCallMatlabMsg(MatlabRPC* msg, T first, Types... args) {
   AssembleCallMatlabMsg(msg, args...);
 }
 
+std::unique_ptr<google::protobuf::io::FileOutputStream>
+CreateOutputStream(const std::string& filename);
+
+void PublishCall(
+    google::protobuf::io::FileOutputStream* praw_output,
+    const MatlabRPC& message);
+
 // Simple wrapper to prevent the outside world from needing to worry about
 // creating a the i/o stream object.
 // TODO(russt): support setting the pipe name (via a

diff --git a/drake/common/proto/call_python.cc b/drake/common/proto/call_python.cc
@@ -0,0 +1,33 @@
+#include "drake/common/proto/call_python.h"
+
+#include "drake/common/proto/matlab_rpc.pb.h"
+
+namespace drake {
+namespace common {
+
+static int py_globally_unique_id = 0;
+
+PythonRemoteVariable::PythonRemoteVariable()
+    : unique_id_(py_globally_unique_id++)
+// TODO(eric.cousineau): Make this consistent with what `call_matlab` has when
+// it changes.
+{}
+
+void ToMatlabArray(const PythonRemoteVariable& var, MatlabArray* matlab_array) {
+  matlab_array->set_type(MatlabArray::REMOTE_VARIABLE_REFERENCE);
+  matlab_array->set_shape_type(MatlabArray::SCALAR);
+  matlab_array->set_rows(1);
+  matlab_array->set_cols(1);
+  int num_bytes = sizeof(int64_t);
+  int64_t uid = var.unique_id();
+  matlab_array->set_data(&uid, num_bytes);
+}
+
+void internal::PublishCallPython(const MatlabRPC& message) {
+  // TODO(eric.cousineau): Provide option for setting the filename.
+  static auto raw_output = CreateOutputStream("/tmp/python_rpc");
+  PublishCall(raw_output.get(), message);
+}
+
+}  // namespace common
+}  // namespace drake