Fixed small issues in evaluations

* FT sensor: fixed a bug in magnitude violation handling
* ManipServer: moved image output height/width to a separate config
  instead of reading from Camera config

Test: tested in experiments

hardware/ati_netft/src/ati_netft.cpp

@@ -133,8 +133,8 @@ int ATINetft::getWrenchSensor(RUT::Vector6d& wrench) {
       std::cout << "  feedback:" << wrench.transpose() << std::endl;
       std::cout << "  safety limit: " << _config.WrenchSafety.transpose()
                 << std::endl;
+      return -1;
     }
-    return -1;
   }
 
   return 0;
@@ -157,12 +157,6 @@ int ATINetft::getWrenchNetTool(const RUT::Vector7d& pose,
   wrench_net_T.head(3) = _wrench_tool_temp.head(3) + _config.Foffset - _GinF;
   wrench_net_T.tail(3) = _wrench_tool_temp.tail(3) + _config.Toffset - _GinT;
 
-  // safety
-  for (int i = 0; i < 6; ++i) {
-    if (abs(wrench_net_T[i]) > _WrenchSafety[i])
-      return 3;
-  }
-
   return flag;
 }
 

workcell/table_top_manip/include/table_top_manip/manip_server.h

@@ -46,6 +46,7 @@ struct ManipServerConfig {
   CameraSelection camera_selection{CameraSelection::NONE};
   ForceSensingMode force_sensing_mode{ForceSensingMode::NONE};
   RUT::Matrix6d low_damping{};
+  std::vector<int> output_rgb_hw{};
 
   bool deserialize(const YAML::Node& node) {
     try {
@@ -68,6 +69,7 @@ struct ManipServerConfig {
 
       low_damping = RUT::deserialize_vector<RUT::Vector6d>(node["low_damping"])
                         .asDiagonal();
+      output_rgb_hw = node["output_rgb_hw"].as<std::vector<int>>();
 
     } catch (const std::exception& e) {
       std::cerr << "Failed to load the config file: " << e.what() << std::endl;

workcell/table_top_manip/src/manip_server.cc

@@ -32,8 +32,6 @@ bool ManipServer::initialize(const std::string& config_path) {
   std::cout << "_id_list: " << _id_list.size() << std::endl;
 
   // parameters to be obtained from config
-  std::vector<int> image_heights;
-  std::vector<int> image_widths;
   std::vector<int> wrench_publish_rate;
 
   std::cout << "[ManipServer] bimanual: " << _config.bimanual << std::endl;
@@ -77,10 +75,6 @@ bool ManipServer::initialize(const std::string& config_path) {
                     << ". Exiting." << std::endl;
           return false;
         }
-        image_heights.push_back(gopro_config.crop_rows[1] -
-                                gopro_config.crop_rows[0]);
-        image_widths.push_back(gopro_config.crop_cols[1] -
-                               gopro_config.crop_cols[0]);
       } else if (_config.camera_selection == CameraSelection::REALSENSE) {
         Realsense::RealsenseConfig realsense_config;
         try {
@@ -99,8 +93,6 @@ bool ManipServer::initialize(const std::string& config_path) {
                     << ". Exiting." << std::endl;
           return false;
         }
-        image_heights.push_back(realsense_config.height);
-        image_widths.push_back(realsense_config.width);
       } else {
         std::cerr << "Invalid camera selection. Exiting." << std::endl;
         return false;
@@ -152,8 +144,6 @@ bool ManipServer::initialize(const std::string& config_path) {
   } else {
     // mock hardware
     for (int id : _id_list) {
-      image_heights.push_back(1080);
-      image_widths.push_back(1080);
       wrench_publish_rate.push_back(7000);
     }
   }
@@ -221,9 +211,9 @@ bool ManipServer::initialize(const std::string& config_path) {
     _waypoints_timestamp_ms_buffers.push_back(RUT::DataBuffer<double>());
     _stiffness_timestamp_ms_buffers.push_back(RUT::DataBuffer<double>());
 
-    _camera_rgb_buffers[id].initialize(_config.rgb_buffer_size,
-                                       3 * image_heights[id], image_widths[id],
-                                       "camera_rgb" + std::to_string(id));
+    _camera_rgb_buffers[id].initialize(
+        _config.rgb_buffer_size, 3 * _config.output_rgb_hw[0],
+        _config.output_rgb_hw[1], "camera_rgb" + std::to_string(id));
 
     _pose_buffers[id].initialize(_config.pose_buffer_size, 7, 1,
                                  "pose" + std::to_string(id));

workcell/table_top_manip/src/manip_server_loops.cc

@@ -334,7 +334,9 @@ void ManipServer::wrench_loop(const RUT::TimePoint& time0, int publish_rate,
         std::lock_guard<std::mutex> lock(_poses_fb_mtxs[id]);
         pose_fb = _poses_fb[id];
       }
-      if (force_sensor_ptrs[id]->getWrenchNetTool(pose_fb, wrench_fb) < 0) {
+      int safety_flag =
+          force_sensor_ptrs[id]->getWrenchNetTool(pose_fb, wrench_fb);
+      if (safety_flag < 0) {
         std::cout << header
                   << "Wrench is above safety threshold. Ending thread."
                   << std::endl;
@@ -422,6 +424,7 @@ void ManipServer::rgb_loop(const RUT::TimePoint& time0, int id) {
   }
   std::cout << header << "Loop started." << std::endl;
 
+  cv::Mat resized_color_mat;
   cv::Mat bgr[3];  //destination array
   Eigen::MatrixXd bm, gm, rm;
   Eigen::MatrixXd rgb_row_combined;
@@ -438,13 +441,16 @@ void ManipServer::rgb_loop(const RUT::TimePoint& time0, int id) {
         usleep(20 * 1000);  // 20ms, 50hz
       }
       time_now_ms = timer.toc_ms();
-      cv::split(_color_mats[id], bgr);  //split source
+      cv::resize(_color_mats[id], resized_color_mat,
+                 cv::Size(_config.output_rgb_hw[1], _config.output_rgb_hw[0]),
+                 cv::INTER_LINEAR);
+      cv::split(resized_color_mat, bgr);  //split source
     }
 
     cv::cv2eigen(bgr[0], bm);
     cv::cv2eigen(bgr[1], gm);
     cv::cv2eigen(bgr[2], rm);
-    rgb_row_combined.resize(_color_mats[id].rows * 3, _color_mats[id].cols);
+    rgb_row_combined.resize(resized_color_mat.rows * 3, resized_color_mat.cols);
     rgb_row_combined << rm, gm, bm;
     {
       std::lock_guard<std::mutex> lock(_camera_rgb_buffer_mtxs[id]);