manip_server: Initial support for bimanual setup

* Change hardware, buffers, etc to vectors
* Change mutex to deque (vector doesn't work on mutex)
* Rename low dim thread to robot thread and separate wrench into a
  separate thread

Test: passed compiling

workcell/table_top_manip/include/table_top_manip/manip_server.h

@@ -2,6 +2,7 @@
 
 #include <unistd.h>
 #include <csignal>
+#include <deque>
 #include <iostream>
 #include <memory>
 #include <mutex>
@@ -33,11 +34,12 @@
 struct ManipServerConfig {
   std::string data_folder{""};
   bool plot_rgb{false};
-  bool run_low_dim_thread{false};
+  bool run_robot_thread{false};
   int rgb_buffer_size{5};
   int pose_buffer_size{100};
   int wrench_buffer_size{100};
   bool mock_hardware{false};
+  bool bimanual{false};
   bool use_perturbation_generator{false};
   CameraSelection camera_selection{CameraSelection::NONE};
   ForceSensingMode force_sensing_mode{ForceSensingMode::NONE};
@@ -46,11 +48,12 @@ struct ManipServerConfig {
     try {
       data_folder = node["data_folder"].as<std::string>();
       plot_rgb = node["plot_rgb"].as<bool>();
-      run_low_dim_thread = node["run_low_dim_thread"].as<bool>();
+      run_robot_thread = node["run_robot_thread"].as<bool>();
       rgb_buffer_size = node["rgb_buffer_size"].as<int>();
       pose_buffer_size = node["pose_buffer_size"].as<int>();
       wrench_buffer_size = node["wrench_buffer_size"].as<int>();
       mock_hardware = node["mock_hardware"].as<bool>();
+      bimanual = node["bimanual"].as<bool>();
       use_perturbation_generator =
           node["use_perturbation_generator"].as<bool>();
       camera_selection = string_to_enum<CameraSelection>(
@@ -99,31 +102,34 @@ class ManipServer {
   bool is_running();
 
   // getters: get the most recent k data points in the buffer
-  const Eigen::MatrixXd get_camera_rgb(int k);
-  const Eigen::MatrixXd get_wrench(int k);
-  const Eigen::MatrixXd get_pose(int k);
+  const Eigen::MatrixXd get_camera_rgb(int k, int camera_id = 0);
+  const Eigen::MatrixXd get_wrench(int k, int sensor_id = 0);
+  const Eigen::MatrixXd get_pose(int k, int robot_id = 0);
 
   // the following functions return the timestamps of
   //  the most recent getter call of the corresponding feedback
   //  So size is already know
-  const Eigen::VectorXd get_camera_rgb_timestamps_ms();
-  const Eigen::VectorXd get_wrench_timestamps_ms();
-  const Eigen::VectorXd get_pose_timestamps_ms();
+  const Eigen::VectorXd get_camera_rgb_timestamps_ms(int id = 0);
+  const Eigen::VectorXd get_wrench_timestamps_ms(int id = 0);
+  const Eigen::VectorXd get_pose_timestamps_ms(int id = 0);
 
   double get_timestamp_now_ms();  // access the current hardware time
 
   void set_high_level_maintain_position();
   void set_high_level_free_jogging();
 
   void set_target_pose(const Eigen::Ref<RUT::Vector7d> pose,
-                       double dt_in_future_ms = 1000);
-  void set_force_controlled_axis(const RUT::Matrix6d& Tr, int n_af);
-  void set_stiffness_matrix(const RUT::Matrix6d& stiffness);
+                       double dt_in_future_ms = 1000, int robot_id = 0);
+  void set_force_controlled_axis(const RUT::Matrix6d& Tr, int n_af,
+                                 int robot_id = 0);
+  void set_stiffness_matrix(const RUT::Matrix6d& stiffness, int robot_id = 0);
 
   void schedule_waypoints(const Eigen::MatrixXd& waypoints,
-                          const Eigen::VectorXd& timepoints_ms);
+                          const Eigen::VectorXd& timepoints_ms,
+                          int robot_id = 0);
   void schedule_stiffness(const Eigen::MatrixXd& stiffness,
-                          const Eigen::VectorXd& timepoints_ms);
+                          const Eigen::VectorXd& timepoints_ms,
+                          int robot_id = 0);
 
   void clear_cmd_buffer();
 
@@ -136,78 +142,88 @@ class ManipServer {
   // config
   ManipServerConfig _config;
 
+  // list of id
+  std::vector<int> _id_list;
+
   // data buffer
-  RUT::DataBuffer<Eigen::MatrixXd> _camera_rgb_buffer;
-  RUT::DataBuffer<Eigen::VectorXd> _pose_buffer;
-  RUT::DataBuffer<Eigen::VectorXd> _wrench_buffer;
+  std::vector<RUT::DataBuffer<Eigen::MatrixXd>> _camera_rgb_buffers;
+  std::vector<RUT::DataBuffer<Eigen::VectorXd>> _pose_buffers;
+  std::vector<RUT::DataBuffer<Eigen::VectorXd>> _wrench_buffers;
   // action buffer
-  RUT::DataBuffer<Eigen::VectorXd> _waypoints_buffer;
-  RUT::DataBuffer<Eigen::MatrixXd> _stiffness_buffer;
+  std::vector<RUT::DataBuffer<Eigen::VectorXd>> _waypoints_buffers;
+  std::vector<RUT::DataBuffer<Eigen::MatrixXd>> _stiffness_buffers;
 
-  RUT::DataBuffer<double> _camera_rgb_timestamp_ms_buffer;
-  RUT::DataBuffer<double> _pose_timestamp_ms_buffer;
-  RUT::DataBuffer<double> _wrench_timestamp_ms_buffer;
-  RUT::DataBuffer<double> _waypoints_timestamp_ms_buffer;
-  RUT::DataBuffer<double> _stiffness_timestamp_ms_buffer;
+  std::vector<RUT::DataBuffer<double>> _camera_rgb_timestamp_ms_buffers;
+  std::vector<RUT::DataBuffer<double>> _pose_timestamp_ms_buffers;
+  std::vector<RUT::DataBuffer<double>> _wrench_timestamp_ms_buffers;
+  std::vector<RUT::DataBuffer<double>> _waypoints_timestamp_ms_buffers;
+  std::vector<RUT::DataBuffer<double>> _stiffness_timestamp_ms_buffers;
 
-  std::mutex _camera_rgb_buffer_mtx;
-  std::mutex _pose_buffer_mtx;
-  std::mutex _wrench_buffer_mtx;
-  std::mutex _waypoints_buffer_mtx;
-  std::mutex _stiffness_buffer_mtx;
+  std::deque<std::mutex> _camera_rgb_buffer_mtxs;
+  std::deque<std::mutex> _pose_buffer_mtxs;
+  std::deque<std::mutex> _wrench_buffer_mtxs;
+  std::deque<std::mutex> _waypoints_buffer_mtxs;
+  std::deque<std::mutex> _stiffness_buffer_mtxs;
 
   // timing
   RUT::Timer _timer;
 
   // additional configs as local variables
-  RUT::Matrix6d _stiffness_high{};
-  RUT::Matrix6d _stiffness_low{};
+  std::vector<RUT::Matrix6d> _stiffnesses_high{};
+  std::vector<RUT::Matrix6d> _stiffnesses_low{};
 
   //  hardware interfaces
-  std::shared_ptr<CameraInterfaces> camera_ptr;
-  std::shared_ptr<FTInterfaces> force_sensor_ptr;
-  std::shared_ptr<RobotInterfaces> robot_ptr;
-  AdmittanceController controller;
-  PerturbationGenerator perturbation_generator;
+  std::vector<std::shared_ptr<CameraInterfaces>> camera_ptrs;
+  std::vector<std::shared_ptr<FTInterfaces>> force_sensor_ptrs;
+  std::vector<std::shared_ptr<RobotInterfaces>> robot_ptrs;
+  // controllers
+  std::vector<AdmittanceController> _controllers;
+  std::vector<PerturbationGenerator> _perturbation_generators;
+  std::deque<std::mutex> _controller_mtxs;
 
   // threads
-  std::thread _rgb_thread;
-  std::thread _low_dim_thread;
-  std::thread _rgb_plot_thread;
+  std::vector<std::thread> _robot_threads;
+  std::vector<std::thread> _wrench_threads;
+  std::vector<std::thread> _rgb_threads;
+  std::vector<std::thread> _rgb_plot_threads;
 
   // control variables to control the threads
-  std::string _ctrl_rgb_folder;
-  std::ofstream _ctrl_low_dim_data_stream;
+  std::vector<std::string> _ctrl_rgb_folders;
+  std::vector<std::ofstream> _ctrl_robot_data_streams;
+  std::vector<std::ofstream> _ctrl_wrench_data_streams;
   bool _ctrl_flag_running = false;  // flag to terminate the program
   bool _ctrl_flag_saving = false;   // flag for ongoing data collection
   std::mutex _ctrl_mtx;
 
-  // controller variables to control the force controller
-  std::mutex _controller_mtx;
-
   // state variable indicating the status of the threads
-  bool _state_low_dim_thread_ready = false;
-  bool _state_rgb_thread_ready = false;
-  bool _state_low_dim_thread_saving = false;
-  bool _state_rgb_thread_saving = false;
-  int _state_low_dim_seq_id = 0;
-  int _state_rgb_seq_id = 0;
-
-  // pre-allocated variables for camera feedback
-  cv::Mat _color_mat;  // also used for plot loop, so need a lock
-  std::mutex _color_mat_mtx;
-  cv::Mat* _bgr;  //destination array
-  Eigen::MatrixXd _bm, _gm, _rm;
-  Eigen::MatrixXd _rgb_row_combined;
+  std::vector<bool> _states_robot_thread_ready{};
+  std::vector<bool> _states_rgb_thread_ready{};
+  std::vector<bool> _states_wrench_thread_ready{};
 
-  // temp variables storing timestamps of data just being fetched
-  Eigen::VectorXd _camera_rgb_timestamps_ms;
-  Eigen::VectorXd _pose_timestamps_ms;
-  Eigen::VectorXd _wrench_timestamps_ms;
+  std::vector<bool> _states_robot_thread_saving{};
+  std::vector<bool> _states_rgb_thread_saving{};
+  std::vector<bool> _states_wrench_thread_saving{};
 
-  void low_dim_loop(const RUT::TimePoint& time0);
+  std::vector<int> _states_robot_seq_id{};
+  std::vector<int> _states_rgb_seq_id{};
+  std::vector<int> _states_wrench_seq_id{};
 
-  void rgb_loop(const RUT::TimePoint& time0);
+  // shared variables between camera thread and plot thread
+  std::vector<cv::Mat> _color_mats;
+  std::deque<std::mutex> _color_mat_mtxs;
 
-  void rgb_plot_loop();
+  // shared variables between robot thread and wrench thread
+  std::vector<Eigen::VectorXd> _poses_fb;
+  std::deque<std::mutex> _poses_fb_mtxs;
+
+  // temp variables storing timestamps of data just being fetched
+  std::vector<Eigen::VectorXd> _camera_rgb_timestamps_ms;
+  std::vector<Eigen::VectorXd> _pose_timestamps_ms;
+  std::vector<Eigen::VectorXd> _wrench_timestamps_ms;
+
+  void robot_loop(const RUT::TimePoint& time0, int robot_id);
+  void rgb_loop(const RUT::TimePoint& time0, int camera_id);
+  void wrench_loop(const RUT::TimePoint& time0, int publish_rate,
+                   int sensor_id);
+  void rgb_plot_loop(int camera_id);
 };

workcell/table_top_manip/src/helpers.hpp

@@ -15,40 +15,65 @@ inline std::string makeFixedLength(const int i, const int length) {
   return ostr.str();
 }
 
-inline std::tuple<std::string, std::string> create_folder_for_new_episode(
-    const std::string& data_folder) {
+inline void create_folder_for_new_episode(
+    const std::string& data_folder, std::vector<int> id_list,
+    std::vector<std::string>& rgb_folders,
+    std::vector<std::string>& robot_json_files,
+    std::vector<std::string>& wrench_json_files) {
   std::cout << "[create_folder_for_new_episode] Creating folder for new episode"
             << std::endl;
   int timestamp = std::chrono::seconds(std::time(NULL)).count();
   const auto timestamp_string = std::to_string(timestamp);
   std::string episode_folder = data_folder + "/episode_" + timestamp_string;
-  std::string rgb_folder = episode_folder + "/rgb";
 
   if (!fs::exists(episode_folder)) {
     fs::create_directory(episode_folder);
-    fs::create_directory(rgb_folder);
   } else {
     std::cerr << "Episode folder " << episode_folder
               << " already exists. Exiting." << std::endl;
     exit(1);
   }
-  std::string data_filename = episode_folder + "/low_dim_data.json";
-  return {rgb_folder, data_filename};
+  rgb_folders.clear();
+  robot_json_files.clear();
+  wrench_json_files.clear();
+  for (int id : id_list) {
+    std::string rgb_folder = episode_folder + "/rgb_" + std::to_string(id);
+    fs::create_directory(rgb_folder);
+    std::string robot_json_file =
+        episode_folder + "/robot_data_" + std::to_string(id) + ".json";
+    std::string wrench_json_file =
+        episode_folder + "/wrench_data_" + std::to_string(id) + ".json";
+    rgb_folders.push_back(rgb_folder);
+    robot_json_files.push_back(robot_json_file);
+    wrench_json_files.push_back(wrench_json_file);
+  }
 }
 
-inline bool save_low_dim_data_json(std::ostream& os, int seq_id,
-                                   double timestamp_ms,
-                                   const RUT::Vector7d& pose,
-                                   const RUT::Vector6d& wrench, bool mask) {
+inline bool save_robot_data_json(std::ostream& os, int seq_id,
+                                 double timestamp_ms, const RUT::Vector7d& pose,
+                                 bool mask) {
   Eigen::IOFormat good_looking_fmt(Eigen::StreamPrecision, Eigen::DontAlignCols,
                                    ", ", ", ", "", "", "", "");
   os << "\t{\n";
   os << "\t\t\"seq_id\": " << seq_id << ",\n";
   os << "\t\t\"mask\": " << mask << ",\n";
-  os << "\t\t\"low_dim_time_stamps\": " << std::fixed << std::setprecision(2)
+  os << "\t\t\"robot_time_stamps\": " << std::fixed << std::setprecision(2)
      << timestamp_ms << ",\n";
   os << std::fixed << std::setprecision(4);
   os << "\t\t\"ts_pose_fb\": [" << pose.format(good_looking_fmt) << "],\n";
+  return true;
+}
+
+inline bool save_wrench_data_json(std::ostream& os, int seq_id,
+                                  double timestamp_ms,
+                                  const RUT::Vector6d& wrench) {
+  Eigen::IOFormat good_looking_fmt(Eigen::StreamPrecision, Eigen::DontAlignCols,
+                                   ", ", ", ", "", "", "", "");
+  os << "\t{\n";
+  os << "\t\t\"seq_id\": " << seq_id << ",\n";
+  os << "\t\t\"wrench_time_stamps\": " << std::fixed << std::setprecision(2)
+     << timestamp_ms << ",\n";
+  os << std::fixed << std::setprecision(4);
   os << "\t\t\"wrench\": [" << wrench.format(good_looking_fmt) << "],\n";
   return true;
 }