Fixed Optoforce DAQ.

app/data/yl_navigation.json

@@ -0,0 +1,504 @@
+{
+  "controller_port": 5277,
+  "variable_list": [
+    {
+      "name": "probe_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "model_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "model_in_model_ref",
+      "type": "transform_obj",
+      "value": [
+        134.705,
+        42.6256,
+        -267.533,
+        0.535693,
+        0.610751,
+        -0.464744,
+        -0.352178
+      ]
+    },
+    {
+      "name": "probe_in_model_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_model",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tool_in_model",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tool_in_robot_flange",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_flange_in_base",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tip_in_model_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_model_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "model_in_model_ref_error",
+      "type": "double_obj"
+    },
+    {
+      "name": "tip_in_model",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_model",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "target_tool_in_model",
+      "type": "transform_obj"
+    },
+    {
+      "name": "target_tool_offset",
+      "type": "transform_obj"
+    },
+    {
+      "name": "target_flange_offset",
+      "type": "transform_obj"
+    },
+    {
+      "name": "target_flange_pose_temp",
+      "type": "transform_obj"
+    },
+    {
+      "name": "flange_move_sig",
+      "type": "bool_obj"
+    },
+    {
+      "name": "target_flange_pose",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_flange_speed_limit",
+      "type": "double_obj",
+      "value": 5
+    },
+    {
+      "name": "kuka_joint_speed_limit",
+      "type": "array7_obj",
+      "value": [
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1
+      ]
+    },
+    {
+      "name": "robot_is_moving",
+      "type": "bool_obj"
+    },
+    {
+      "name": "kuka_control_mode",
+      "type": "u64int_obj"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "transform_tree",
+      "name": "transform_tree",
+      "init_config": {
+        "node_list": [
+          {
+            "name": "tracker"
+          },
+          {
+            "name": "probe",
+            "parent": "tracker",
+            "transform_var_name": "probe_in_tracker"
+          },
+          {
+            "name": "model_ref",
+            "parent": "tracker",
+            "transform_var_name": "model_ref_in_tracker"
+          },
+          {
+            "name": "model",
+            "parent": "model_ref",
+            "transform_var_name": "model_in_model_ref"
+          },
+          {
+            "name": "robot_base",
+            "parent": "tracker",
+            "transform": [
+              227.964774380901,
+              408.415090613534,
+              -2083.90135385252,
+              0.718310380441512,
+              0.277543700520970,
+              -0.563300905075462,
+              -0.299485862758597
+            ]
+          },
+          {
+            "name": "robot_flange",
+            "parent": "robot_base",
+            "transform_var_name": "robot_flange_in_base"
+          },
+          {
+            "name": "tool_real",
+            "parent": "robot_flange",
+            "transform": [
+              193.674181012287,
+              119.007940358412,
+              0.975841097441376,
+              0.340960180366781,
+              -0.343553889200713,
+              -0.619099078363195,
+              0.618411846417222
+            ]
+          },
+          {
+            "name": "tool",
+            "parent": "tool_real",
+            "transform": [
+              6,
+              5,
+              -25,
+              1,
+              0,
+              0,
+              0
+            ]
+          }
+        ]
+      },
+      "start_config": {
+        "watch_list": [
+          {
+            "target": "probe",
+            "observer": "model_ref",
+            "transform_var_name": "probe_in_model_ref"
+          },
+          {
+            "target": "probe",
+            "observer": "model",
+            "transform_var_name": "probe_in_model"
+          },
+          {
+            "target": "tool",
+            "observer": "model",
+            "transform_var_name": "tool_in_model"
+          },
+          {
+            "target": "tool",
+            "observer": "robot_flange",
+            "transform_var_name": "tool_in_robot_flange"
+          }
+        ]
+      }
+    },
+    {
+      "type": "ndi_interface",
+      "name": "ndi",
+      "init_config": {
+        "address_type": "ethernet",
+        "ip": "10.0.0.5",
+        "tcp_port": 8765,
+        "com_port": "COM3",
+        "tool_list": [
+          {
+            "rom_path": "/home/tpx/data/roms/Probe_Small_4Ball.rom",
+            "outputs": {
+              "transform": "probe_in_tracker"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/ZHAN2210.rom",
+            "outputs": {
+              "transform": "model_ref_in_tracker"
+            }
+          }
+        ]
+      },
+      "start_config": {
+        "allow_unreliable": true,
+        "prefer_stream_tracking": false
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "probe_visibility_watcher",
+      "start_config": {
+        "variable_name": "probe_in_tracker"
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "model_ref_visibility_watcher",
+      "start_config": {
+        "variable_name": "model_ref_in_tracker"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_transformer_for_reg",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_model_ref",
+        "target_value": [
+          0,
+          0,
+          0
+        ],
+        "output_var_name": "tip_in_model_ref"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker_for_reg",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "tip_in_model_ref",
+        "output_var_name": "picked_point_in_model_ref",
+        "linear_tolerance_mm": 0.05,
+        "temporal_interval_s": 3
+      },
+      "dependencies": [
+        "probe_tip_transformer_for_reg"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher_for_reg",
+      "start_config": {
+        "variable_name": "picked_point_in_model_ref"
+      }
+    },
+    {
+      "type": "landmark_registration",
+      "name": "model_registrator",
+      "start_config": {
+        "fiducial_points": [
+          [
+            2.923151011148235,
+            215.9872333064639,
+            42.679712305162866
+          ],
+          [
+            1.2261328201978756,
+            219.42019907958326,
+            14.428129469734642
+          ],
+          [
+            2.172500957424404,
+            224.70318274606566,
+            6.2830603935697695
+          ],
+          [
+            -20.58358250552783,
+            254.10731512319853,
+            -22.473281041750283
+          ]
+        ],
+        "point_var_name": "picked_point_in_model_ref",
+        "transform_var_name": "model_in_model_ref",
+        "error_var_name": "model_in_model_ref_error"
+      },
+      "dependencies": [
+        "point_picker_for_reg"
+      ]
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "registration_result_watcher",
+      "start_config": {
+        "variable_name": "model_in_model_ref"
+      }
+    },
+    {
+      "type": "double_obj_watcher",
+      "name": "registration_error_watcher",
+      "start_config": {
+        "variable_name": "model_in_model_ref_error"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_transformer_for_nav",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_model",
+        "target_value": [
+          0,
+          0,
+          0
+        ],
+        "output_var_name": "tip_in_model"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker_for_icp",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "tip_in_model",
+        "output_var_name": "picked_point_in_model",
+        "linear_tolerance_mm": 0.05,
+        "temporal_interval_s": 2
+      },
+      "dependencies": [
+        "probe_tip_transformer_for_nav"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher_for_icp",
+      "start_config": {
+        "variable_name": "picked_point_in_model"
+      }
+    },
+    {
+      "type": "kuka_interface",
+      "name": "kuka",
+      "init_config": {
+        "address": "10.0.0.10"
+      },
+      "start_config": {
+        "output_config": {
+          "report_frequency": 60,
+          "flange_info": {
+            "pose": "robot_flange_in_base"
+          },
+          "is_moving": "robot_is_moving",
+          "current_control_mode": "kuka_control_mode"
+        },
+        "input_config": {
+          "control_mode": "hard_flange_pose",
+          "flange_pose": "target_flange_pose",
+          "flange_speed_limit": "robot_flange_speed_limit",
+          "joint_speed_limit": "kuka_joint_speed_limit"
+        }
+      }
+    },
+    {
+      "type": "five_dof_offset_calculator",
+      "name": "tcp_offset_calculator",
+      "start_config": {
+        "current_transform_var": "tool_in_model",
+        "target_transform_var": "target_tool_in_model",
+        "offset_transform_var": "target_tool_offset"
+      },
+      "dependencies": [
+        "kuka",
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "tcp_offset_to_flange_calculator",
+      "name": "flange_offset_calculator",
+      "start_config": {
+        "tcp_offset_transform_var": "target_tool_offset",
+        "tcp_in_flange_transform_var": "tool_in_robot_flange",
+        "flange_offset_transform_var": "target_flange_offset"
+      },
+      "dependencies": [
+        "kuka",
+        "tcp_offset_calculator"
+      ]
+    },
+    {
+      "type": "simple_offset_to_pose_controller",
+      "name": "kuka_controller",
+      "start_config": {
+        "current_transform_var": "robot_flange_in_base",
+        "offset_transform_var": "target_flange_offset",
+        "target_transform_var": "target_flange_pose_temp"
+      },
+      "dependencies": [
+        "kuka",
+        "flange_offset_calculator"
+      ]
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "robot_old_flange_watcher",
+      "start_config": {
+        "variable_name": "robot_flange_in_base",
+        "minimum_interval_ms": 2000
+      }
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "robot_new_flange_watcher",
+      "start_config": {
+        "variable_name": "target_flange_pose_temp",
+        "minimum_interval_ms": 2000
+      }
+    },
+    {
+      "type": "transform_obj_d_latch",
+      "name": "kuka_controller_latch",
+      "start_config": {
+        "input_name": "target_flange_pose_temp",
+        "output_name": "target_flange_pose",
+        "signal_name": "flange_move_sig"
+      },
+      "dependencies": [
+        "kuka",
+        "kuka_controller"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "reg_model",
+      "dependencies": [
+        "probe_visibility_watcher",
+        "model_ref_visibility_watcher",
+        "picked_point_watcher_for_reg",
+        "model_registrator",
+        "registration_result_watcher",
+        "registration_error_watcher"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "icp_model",
+      "dependencies": [
+        "probe_visibility_watcher",
+        "model_ref_visibility_watcher",
+        "picked_point_watcher_for_icp",
+        "point_picker_for_icp",
+        "registration_result_watcher"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "robot",
+      "dependencies": [
+        "kuka_controller_latch",
+        "robot_old_flange_watcher",
+        "robot_new_flange_watcher"
+      ]
+    }
+  ]
+}

src/core/global_defs.cpp

@@ -30,6 +30,12 @@ DEFAULT_TRISTATE_OBJ_DEF(ndi_interface)
 
 #endif
 
+#if !SOPHIAR_TEST || SOPHIAR_TEST_SENSOR
+
+DEFAULT_TRISTATE_OBJ_DEF(optoforce_daq)
+
+#endif
+
 namespace sophiar {
 
     dynamic_pool *global_dynamic_pool = nullptr;
@@ -101,6 +107,12 @@ namespace sophiar {
 
         REGISTER_TYPE(ndi_interface);
 
+#endif
+
+#if !SOPHIAR_TEST || SOPHIAR_TEST_SENSOR
+
+        REGISTER_TYPE(optoforce_daq);
+
 #endif
 
         register_variable_utility_types();

src/sensor/optoforce/optoforce_daq.cpp

@@ -1,4 +1,3 @@
-#include "optoforce_daq.h"
 #include "core/basic_obj_types.hpp"
 #include "utility/config_utility.hpp"
 #include "utility/coro_worker.hpp"
@@ -14,6 +13,10 @@
 
 #include <numeric>
 
+#include "utility/assert_utility.h"
+
+DEFAULT_TRISTATE_OBJ_DEF(optoforce_daq)
+
 namespace sophiar {
 
     using boost::asio::async_read;
@@ -48,7 +51,8 @@ namespace sophiar {
 
         template<size_t MessageLength>
         uint16_t calc_checksum(const char *data) {
-            return std::accumulate(data, data + MessageLength - 2, static_cast<uint16_t>(0));
+            auto real_ptr = (const uint8_t *) data;
+            return std::accumulate(real_ptr, real_ptr + MessageLength - 2, static_cast<uint16_t>(0));
         }
 
         template<size_t MessageLength>
@@ -83,7 +87,7 @@ namespace sophiar {
 
             // force component
             auto ts = current_timestamp();
-            uint16_t fx, fy, fz;
+            int16_t fx, fy, fz;
             reader >> fx >> fy >> fz;
             auto force_value = Eigen::Vector3d(
                     fx * force_resolution,
@@ -92,7 +96,7 @@ namespace sophiar {
             UPDATE_VARIABLE_VAL_WITH_TS(scalarxyz_obj, force_var_index, std::move(force_value), ts);
 
             // torque component
-            uint16_t tx, ty, tz;
+            int16_t tx, ty, tz;
             reader >> tx >> ty >> tz;
             auto torque_value = Eigen::Vector3d(
                     tx * torque_resolution,
@@ -169,7 +173,7 @@ namespace sophiar {
             switch (device_type) {
                 case 3: {
                     auto func_wrapper = make_noexcept_func(
-                            std::bind(&impl::handle_type_3_message, this));
+                            [this]() { return handle_type_3_message(); });
                     worker = make_infinite_coro_worker(std::move(func_wrapper), std::move(exit_func));
                     break;
                 }
@@ -200,16 +204,14 @@ namespace sophiar {
             co_await pimpl->load_device_config(config);
             pimpl->create_worker();
         } catch (std::exception &e) {
-            // TODO show log
+            SPDLOG_ERROR("Failed to start Optoforce DAQ: {}", e.what());
             co_return false;
         }
         co_return true;
     }
 
     awaitable<void> optoforce_daq::on_stop() noexcept {
-        pimpl->worker->cancel();
-        co_await pimpl->worker->coro_wait_stop();
-        pimpl->worker = nullptr;
+        SAFE_RESET_WORKER(pimpl->worker)
         co_return;
     }
 

src/sensor/optoforce/optoforce_daq.h

@@ -1,8 +0,0 @@
-#ifndef SOPHIAR2_OPTOFORCE_DAQ_H
-#define SOPHIAR2_OPTOFORCE_DAQ_H
-
-#include "core/tristate_obj.h"
-
-DEFAULT_TRISTATE_OBJ_DEF(optoforce_daq)
-
-#endif //SOPHIAR2_OPTOFORCE_DAQ_H

tests/CMakeLists.txt

@@ -45,4 +45,13 @@ add_executable(test_ndi
         ${CORE_IMPL_FILES})
 target_compile_definitions(test_ndi PUBLIC SOPHIAR_TEST SOPHIAR_TEST_NDI SOPHIAR_TEST_ALGORITHM)
 #target_compile_definitions(test_ndi PUBLIC CORO_SIGNAL2_USE_TIMER)
-target_link_libraries(test_ndi ${Boost_LIBRARIES} ${EXTRA_LIBS})
+target_link_libraries(test_ndi ${Boost_LIBRARIES} ${EXTRA_LIBS})
+
+file(GLOB_RECURSE TEST_SENSOR_SRC_FILES ./sensor/*.cpp)
+add_executable(test_sensor
+        ${TEST_SENSOR_SRC_FILES}
+        ../src/sensor/optoforce/optoforce_daq.cpp
+        ${CORE_IMPL_FILES})
+target_compile_definitions(test_sensor PUBLIC SOPHIAR_TEST SOPHIAR_TEST_SENSOR)
+#target_compile_definitions(test_ndi PUBLIC CORO_SIGNAL2_USE_TIMER)
+target_link_libraries(test_sensor ${Boost_LIBRARIES} ${EXTRA_LIBS})

tests/data/optoforce_daq_config.json

@@ -1,11 +1,21 @@
 {
   "listen_port": 5277,
+  "variable_list": [
+    {
+      "name": "flange_force",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "flange_torque",
+      "type": "scalarxyz_obj"
+    }
+  ],
   "object_list": [
     {
       "type": "optoforce_daq",
       "name": "optoforce_daq",
       "start_config": {
-        "com_port": "COM5",
+        "com_port": "/dev/ttyACM0",
         "device_type": 3,
         "force_output_name": "flange_force",
         "torque_output_name": "flange_torque",
@@ -14,6 +24,31 @@
         "report_frequency": 100,
         "filter_type": 4
       }
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "force_watcher",
+      "start_config": {
+        "minimum_interval_ms": 500,
+        "variable_name": "flange_force"
+      }
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "torque_watcher",
+      "start_config": {
+        "minimum_interval_ms": 500,
+        "variable_name": "flange_torque"
+      }
+    },
+    {
+      "type": "empty_object",
+      "name": "all",
+      "dependencies": [
+        "optoforce_daq",
+        "force_watcher",
+        "torque_watcher"
+      ]
     }
   ]
 }

tests/sensor/optoforce_daq.cpp

@@ -0,0 +1,23 @@
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MAIN  // in only one cpp file
+
+#include "core/global_defs.h"
+
+#include <boost/test/unit_test.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include <fstream>
+
+using namespace sophiar;
+using namespace std::chrono_literals;
+
+BOOST_AUTO_TEST_CASE(test_optoforce_daq) {
+    spdlog::set_level(spdlog::level::trace);
+    std::ifstream config_file("data/optoforce_daq_config.json");
+    BOOST_TEST(config_file.is_open());
+    auto config = nlohmann::json::parse(config_file);
+    auto ok = initialize(config);
+    BOOST_TEST(ok);
+    global_context->run();
+}