added time stamp to robot states

CMakeLists.txt

@@ -86,6 +86,9 @@ add_library(${PROJECT_NAME}
     src/async_port/async_serial.cpp
     src/async_port/async_can.cpp
     ########################
+    ## utilities
+    src/utilities/protocol_detector.cpp
+    ########################
     ## public interface to access robot
     src/mobile_robot/scout_robot.cpp
     src/mobile_robot/hunter_robot.cpp

demo/CMakeLists.txt

@@ -4,3 +4,5 @@ add_subdirectory(tracer_demo)
 add_subdirectory(ranger_demo)
 add_subdirectory(bunker_demo)
 add_subdirectory(hunter_demo)
+
+add_subdirectory(utils_demo)

demo/bunker_demo/CMakeLists.txt

@@ -1,3 +1,2 @@
-# tests
 add_executable(demo_bunker_robot bunker_robot_demo.cpp)
 target_link_libraries(demo_bunker_robot ugv_sdk)

demo/hunter_demo/CMakeLists.txt

@@ -1,3 +1,2 @@
-# tests
 add_executable(demo_hunter_robot hunter_robot_demo.cpp)
 target_link_libraries(demo_hunter_robot ugv_sdk)

demo/hunter_demo/hunter_demo.cpp

@@ -1,92 +0,0 @@
-/* 
- * demo_hunter_can.cpp
- * 
- * Created on: Jun 12, 2019 05:03
- * Description: 
- * 
- * Copyright (c) 2019 Ruixiang Du (rdu)
- */
-
-#include "ugv_sdk/hunter_base.hpp"
-
-using namespace westonrobot;
-
-int main(int argc, char **argv)
-{
-    std::string device_name;
-    int32_t baud_rate = 0;
-
-    if (argc == 2)
-    {
-        device_name = {argv[1]};
-        std::cout << "Specified CAN: " << device_name << std::endl;
-    }
-    else
-    {
-        std::cout << "Usage: app_hunter_demo <interface>" << std::endl
-                  << "Example 1: ./app_hunter_demo can0" << std::endl;
-        return -1;
-    }
-
-    HunterBase hunter;
-    hunter.Connect(device_name);
-
-    int count = 0;
-    while (true)
-    {
-        // motion control
-        if (count < 5)
-        {
-            std::cout << "Motor: 0.2, 0.0" << std::endl;
-            hunter.SetMotionCommand(0.2, 0.0);
-        }
-        else if (count < 10)
-        {
-        std::cout << "Motor: 0.8, 0.3" << std::endl;
-        hunter.SetMotionCommand(0.8, 0.3);
-        }
-        else if (count < 15)
-        {
-            std::cout << "Motor: 1.5, 0.5" << std::endl;
-            hunter.SetMotionCommand(1.5, 0.5);
-        }
-        else if (count < 20)
-        {
-            std::cout << "Motor: 1.0, 0.3" << std::endl;
-            hunter.SetMotionCommand(1.0, 0.3);
-        }
-        else if (count < 25)
-        {
-            std::cout << "Motor: 0.0, 0.0" << std::endl;
-            hunter.SetMotionCommand(0.0, 0.0);
-        }
-        else if (count < 30)
-        {
-            std::cout << "Motor: -0.5, -0.3" << std::endl;
-            hunter.SetMotionCommand(-0.5, -0.3);
-        }
-        else if (count < 35)
-        {
-            std::cout << "Motor: -1.0, -0.5" << std::endl;
-            hunter.SetMotionCommand(-1.0, -0.5);
-        }
-        else if (count < 40)
-        {
-            std::cout << "Motor: 0.0, 0.0," << std::endl;
-            hunter.SetMotionCommand(0.0, 0.0);
-        }
-
-        auto state = hunter.GetHunterState();
-        std::cout << "-------------------------------" << std::endl;
-        std::cout << "count: " << count << std::endl;
-        std::cout << "control mode: " << static_cast<int>(state.control_mode) << " , base state: " << static_cast<int>(state.base_state) << std::endl;
-        std::cout << "battery voltage: " << state.battery_voltage << std::endl;
-        std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.steering_angle << std::endl;
-        std::cout << "-------------------------------" << std::endl;
-
-        sleep(1);
-        ++count;
-    }
-
-    return 0;
-}

demo/ranger_demo/CMakeLists.txt

@@ -1,2 +1,2 @@
-add_executable(demo_ranger_robot ranger_demo.cpp)
+add_executable(demo_ranger_robot ranger_robot_demo.cpp)
 target_link_libraries(demo_ranger_robot ugv_sdk)

demo/scout_demo/CMakeLists.txt

@@ -1,6 +1,2 @@
-# tests
-add_executable(demo_scout_v2 scout_v2_demo.cpp)
-target_link_libraries(demo_scout_v2 ugv_sdk)
-
 add_executable(demo_scout_robot scout_robot_demo.cpp)
 target_link_libraries(demo_scout_robot ugv_sdk)

demo/scout_demo/scout_robot_demo.cpp

@@ -13,34 +13,38 @@
 #include <iostream>
 
 #include "ugv_sdk/mobile_robot/scout_robot.hpp"
+#include "ugv_sdk/utilities/protocol_detector.hpp"
 
 using namespace westonrobot;
 
 int main(int argc, char **argv) {
-  std::string protocol_version;
   std::string device_name;
 
-  if (argc == 3) {
+  if (argc == 2) {
-    protocol_version = {argv[1]};
+    device_name = {argv[1]};
-    device_name = {argv[2]};
+    std::cout << "Selected interface " << device_name << std::endl;
-    std::cout << "Use protocol " << protocol_version << " on interface "
-              << device_name << std::endl;
   } else {
     std::cout << "Usage: app_scout_demo <protocol-version> <interface>"
               << std::endl
-              << "Example 1: ./app_scout_demo v1 can0" << std::endl;
+              << "Example 1: ./app_scout_demo can0" << std::endl;
     return -1;
   }
 
   std::unique_ptr<ScoutRobot> scout;
-  if (protocol_version == "v1") {
+
+  ProtocolDectctor detector;
+  detector.Connect("can0");
+  auto proto = detector.DetectProtocolVersion(5);
+  if (proto == ProtocolVersion::AGX_V1) {
+    std::cout << "Detected protocol: AGX_V1" << std::endl;
     scout = std::unique_ptr<ScoutRobot>(
         new ScoutRobot(ProtocolVersion::AGX_V1, true));
-  } else if (protocol_version == "v2") {
+  } else if (proto == ProtocolVersion::AGX_V2) {
+    std::cout << "Detected protocol: AGX_V2" << std::endl;
     scout = std::unique_ptr<ScoutRobot>(
         new ScoutRobot(ProtocolVersion::AGX_V2, true));
   } else {
-    std::cout << "Error: invalid protocol version string" << std::endl;
+    std::cout << "Detected protocol: UNKONWN" << std::endl;
     return -1;
   }
 
@@ -51,9 +55,6 @@ int main(int argc, char **argv) {
 
   if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V2) {
     scout->EnableCommandedMode();
-    std::cout << "Protocol version 2" << std::endl;
-  } else {
-    std::cout << "Protocol version 1" << std::endl;
   }
 
   // light control
@@ -66,11 +67,9 @@ int main(int argc, char **argv) {
   std::cout << "Light: breath" << std::endl;
   scout->SetLightCommand(BREATH, 0, BREATH, 0);
   sleep(3);
-  std::cout << "Light: custom 30-80" << std::endl;
+  std::cout << "Light: custom 30-40" << std::endl;
-  scout->SetLightCommand(CUSTOM, 30, CUSTOM, 80);
+  scout->SetLightCommand(CUSTOM, 30, CUSTOM, 40);
   sleep(3);
-  //   std::cout << "Light: diabled cmd control" << std::endl;
-  //   scout->DisableLightControl();
   scout->SetLightCommand(CONST_OFF, 0, CONST_OFF, 0);
 
   int count = 0;
@@ -88,11 +87,17 @@ int main(int argc, char **argv) {
               << " , vehicle state: "
               << static_cast<int>(state.system_state.vehicle_state)
               << " , error code: " << std::hex << state.system_state.error_code
+              << std::dec
               << ", battery voltage: " << state.system_state.battery_voltage
               << std::endl;
     std::cout << "velocity (linear, angular): "
               << state.motion_state.linear_velocity << ", "
               << state.motion_state.angular_velocity << std::endl;
+    std::cout << "core state age (ms): "
+              << std::chrono::duration_cast<std::chrono::milliseconds>(
+                     AgxMsgRefClock::now() - state.time_stamp)
+                     .count()
+              << std::endl;
 
     auto actuator = scout->GetActuatorState();
     if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V1) {
@@ -104,7 +109,25 @@ int main(int argc, char **argv) {
                actuator.actuator_state[i].driver_temp,
                actuator.actuator_state[i].motor_temp);
       }
+      std::cout << "actuator state age (ms): "
+                << std::chrono::duration_cast<std::chrono::milliseconds>(
+                       AgxMsgRefClock::now() - actuator.time_stamp)
+                       .count()
+                << std::endl;
     } else {
+      for (int i = 0; i < 4; ++i) {
+        printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n",
+               actuator.actuator_hs_state[i].motor_id,
+               actuator.actuator_hs_state[i].current,
+               actuator.actuator_hs_state[i].rpm,
+               actuator.actuator_ls_state[i].driver_temp,
+               actuator.actuator_ls_state[i].motor_temp);
+      }
+      std::cout << "actuator state age (ms): "
+                << std::chrono::duration_cast<std::chrono::milliseconds>(
+                       AgxMsgRefClock::now() - actuator.time_stamp)
+                       .count()
+                << std::endl;
     }
     std::cout << "-------------------------------" << std::endl;
 

demo/scout_demo/scout_v2_demo.cpp

@@ -1,75 +0,0 @@
-/*
- * demo_scout_can.cpp
- *
- * Created on: Jun 12, 2019 05:03
- * Description:
- *
- * Copyright (c) 2019 Ruixiang Du (rdu)
- */
-
-#include "ugv_sdk/details/robot_base/scout_base.hpp"
-
-using namespace westonrobot;
-
-int main(int argc, char **argv) {
-  std::string device_name;
-
-  if (argc == 2) {
-    device_name = {argv[1]};
-    std::cout << "Specified CAN: " << device_name << std::endl;
-  } else {
-    std::cout << "Usage: app_scout_demo <interface>" << std::endl
-              << "Example 1: ./app_scout_demo can0" << std::endl;
-    return -1;
-  }
-
-  ScoutBaseV2 scout;
-  scout.Connect(device_name);
-
-  scout.EnableCommandedMode();
-
-  //   // light control
-  std::cout << "Light: const off" << std::endl;
-  scout.SetLightCommand(CONST_OFF, 0, CONST_OFF, 0);
-  sleep(3);
-  std::cout << "Light: const on" << std::endl;
-  scout.SetLightCommand(CONST_ON, 0, CONST_ON, 0);
-  sleep(3);
-  std::cout << "Light: breath" << std::endl;
-  scout.SetLightCommand(BREATH, 0, BREATH, 0);
-  sleep(3);
-  std::cout << "Light: custom 90-80" << std::endl;
-  scout.SetLightCommand(CUSTOM, 90, CUSTOM, 80);
-  sleep(3);
-  std::cout << "Light: diabled cmd control" << std::endl;
-  scout.DisableLightControl();
-
-  int count = 0;
-  while (true) {
-    // motion control
-    if (count < 500) {
-      std::cout << "Motor: 0.2, 0" << std::endl;
-      scout.SetMotionCommand(0.2, 0.0);
-    }
-
-    auto state = scout.GetRobotState();
-    std::cout << "-------------------------------" << std::endl;
-    std::cout << "count: " << count << std::endl;
-    std::cout << "control mode: "
-              << static_cast<int>(state.system_state.control_mode)
-              << " , vehicle state: "
-              << static_cast<int>(state.system_state.vehicle_state)
-              << std::endl;
-    std::cout << "battery voltage: " << state.system_state.battery_voltage
-              << std::endl;
-    std::cout << "velocity (linear, angular): "
-              << state.motion_state.linear_velocity << ", "
-              << state.motion_state.angular_velocity << std::endl;
-    std::cout << "-------------------------------" << std::endl;
-
-    usleep(20000);
-    ++count;
-  }
-
-  return 0;
-}

demo/tracer_demo/tracer_demo.cpp

@@ -1,99 +0,0 @@
-/*
- * demo_tracer_can.cpp
- *
- * Created on: Jun 12, 2019 05:03
- * Description:
- *
- * Copyright (c) 2019 Ruixiang Du (rdu)
- */
-
-#include "ugv_sdk/protocol_v2/tracer_base.hpp"
-
-using namespace westonrobot;
-
-int main(int argc, char **argv) {
-  std::string device_name;
-
-  if (argc == 2) {
-    device_name = {argv[1]};
-    std::cout << "Specified CAN: " << device_name << std::endl;
-  } else {
-    std::cout << "Usage: app_tracer_demo <interface>" << std::endl
-              << "Example 1: ./app_tracer_demo can0" << std::endl;
-    return -1;
-  }
-
-  TracerBaseV2 tracer;
-  tracer.Connect(device_name);
-
-  tracer.EnableCommandedMode();
-  //   tracer.DisableTimeout();
-
-  // light control
-  std::cout << "Light: const off" << std::endl;
-  tracer.SetLightCommand({CONST_OFF, 0});
-  //   usleep(50000);
-  sleep(3);
-  std::cout << "Light: const on" << std::endl;
-  tracer.SetLightCommand({CONST_ON, 0});
-  //   usleep(50000);
-  sleep(3);
-  std::cout << "Light: breath" << std::endl;
-  tracer.SetLightCommand({BREATH, 0});
-  //   usleep(50000);
-  sleep(8);
-  std::cout << "Light: custom 90-80" << std::endl;
-  tracer.SetLightCommand({CUSTOM, 90});
-  //   usleep(50000);
-  sleep(3);
-  std::cout << "Light: diabled cmd control" << std::endl;
-  tracer.SetLightCommand(TracerLightCmd());
-
-  int count = 0;
-  while (true) {
-    // motion control
-    if (count < 5) {
-      std::cout << "Motor: 0.2, 0.0" << std::endl;
-      tracer.SetMotionCommand(0.2, 0.0);
-    } else if (count < 10) {
-      std::cout << "Motor: 0.8, 0.3" << std::endl;
-      tracer.SetMotionCommand(0.8, 0.3);
-    } else if (count < 15) {
-      std::cout << "Motor: 1.5, 0.5" << std::endl;
-      tracer.SetMotionCommand(1.5, 0.5);
-    } else if (count < 20) {
-      std::cout << "Motor: 1.0, 0.3" << std::endl;
-      tracer.SetMotionCommand(1.0, 0.3);
-    } else if (count < 25) {
-      std::cout << "Motor: 0.0, 0.0" << std::endl;
-      tracer.SetMotionCommand(0.0, 0.0);
-    } else if (count < 30) {
-      std::cout << "Motor: -0.5, -0.3" << std::endl;
-      tracer.SetMotionCommand(-0.5, -0.3);
-    } else if (count < 35) {
-      std::cout << "Motor: -1.0, -0.5" << std::endl;
-      tracer.SetMotionCommand(-1.0, -0.5);
-    } else if (count < 40) {
-      std::cout << "Motor: 0.0, 0.0," << std::endl;
-      tracer.SetMotionCommand(0.0, 0.0);
-    }
-    // tracer.SetMotionCommand(0.8, 0.8);
-
-    auto state = tracer.GetTracerState();
-    std::cout << "-------------------------------" << std::endl;
-    std::cout << "count: " << count << std::endl;
-    std::cout << "control mode: " << static_cast<int>(state.system_state.control_mode)
-              << " , vehicle state: " << static_cast<int>(state.system_state.vehicle_state)
-              << std::endl;
-    std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl;
-    std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", "
-              << state.motion_state.angular_velocity << std::endl;
-    std::cout << "-------------------------------" << std::endl;
-
-    // usleep(20000);
-    sleep(1);
-    ++count;
-  }
-
-  return 0;
-}

demo/utils_demo/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_executable(demo_protocol_detector demo_protocol_detector.cpp)
+target_link_libraries(demo_protocol_detector ugv_sdk)

demo/utils_demo/demo_protocol_detector.cpp

@@ -0,0 +1,32 @@
+/*
+ * demo_protocol_detect.cpp
+ *
+ * Created on: Jul 15, 2021 14:10
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#include <memory>
+#include <iostream>
+
+#include "ugv_sdk/utilities/protocol_detector.hpp"
+
+using namespace westonrobot;
+
+int main(int argc, char **argv) {
+  ProtocolDectctor detector;
+  detector.Connect("can0");
+
+  auto proto = detector.DetectProtocolVersion(5);
+
+  if (proto == ProtocolVersion::AGX_V1) {
+    std::cout << "Detected protocol: AGX_V1" << std::endl;
+  } else if (proto == ProtocolVersion::AGX_V2) {
+    std::cout << "Detected protocol: AGX_V2" << std::endl;
+  } else {
+    std::cout << "Detected protocol: UNKONWN" << std::endl;
+  }
+
+  return 0;
+}

include/ugv_sdk/details/interface/agilex_message.h

@@ -119,8 +119,8 @@ typedef struct {
 typedef struct {
   uint8_t motor_id;
   float driver_voltage;
-  float driver_temperature;
+  float driver_temp;
-  int8_t motor_temperature;
+  float motor_temp;
   uint8_t driver_state;
 } ActuatorLSStateMessage;
 

include/ugv_sdk/details/interface/bunker_interface.hpp

@@ -13,9 +13,12 @@
 #include <string>
 
 #include "ugv_sdk/details/interface/agilex_message.h"
+#include "ugv_sdk/details/interface/robot_common_interface.hpp"
 
 namespace westonrobot {
 struct BunkerCoreState {
+  AgxMsgTimeStamp time_stamp;
+
   SystemStateMessage system_state;
   MotionStateMessage motion_state;
   RcStateMessage rc_state;

include/ugv_sdk/details/interface/hunter_interface.hpp

@@ -13,16 +13,20 @@
 #include <string>
 
 #include "ugv_sdk/details/interface/agilex_message.h"
+#include "ugv_sdk/details/interface/robot_common_interface.hpp"
 
 namespace westonrobot {
 struct HunterCoreState {
+  AgxMsgTimeStamp time_stamp;
+
   SystemStateMessage system_state;
   MotionStateMessage motion_state;
-  LightStateMessage light_state;
   RcStateMessage rc_state;
 };
 
 struct HunterActuatorState {
+  AgxMsgTimeStamp time_stamp;
+
   // actuator state
   ActuatorHSStateMessage actuator_hs_state[3];
   ActuatorLSStateMessage actuator_ls_state[3];

include/ugv_sdk/details/interface/ranger_interface.hpp

@@ -13,10 +13,13 @@
 #include <string>
 
 #include "ugv_sdk/details/interface/agilex_message.h"
+#include "ugv_sdk/details/interface/robot_common_interface.hpp"
 
 namespace westonrobot {
 struct RangerCoreState {
   // system state
+  AgxMsgTimeStamp time_stamp;
+
   SystemStateMessage system_state;
   MotionStateMessage motion_state;
   LightStateMessage light_state;
@@ -27,6 +30,8 @@ struct RangerCoreState {
 };
 
 struct RangerActuatorState {
+  AgxMsgTimeStamp time_stamp;
+
   ActuatorHSStateMessage actuator_hs_state[8];
   ActuatorLSStateMessage actuator_ls_state[8];
 };

include/ugv_sdk/details/interface/robot_common_interface.hpp

@@ -11,6 +11,7 @@
 #define ROBOT_INTERFACE_HPP
 
 #include <string>
+#include <chrono>
 
 #include "ugv_sdk/details/interface/agilex_message.h"
 #include "ugv_sdk/details/interface/parser_interface.hpp"
@@ -18,7 +19,12 @@
 #define AGX_MAX_ACTUATOR_NUM 8
 
 namespace westonrobot {
+using AgxMsgRefClock = std::chrono::steady_clock;
+using AgxMsgTimeStamp = std::chrono::time_point<AgxMsgRefClock>;
+
 struct CoreStateMsgGroup {
+  AgxMsgTimeStamp time_stamp;
+
   SystemStateMessage system_state;
   MotionStateMessage motion_state;
   LightStateMessage light_state;
@@ -27,6 +33,8 @@ struct CoreStateMsgGroup {
 };
 
 struct ActuatorStateMsgGroup {
+  AgxMsgTimeStamp time_stamp;
+
   ActuatorHSStateMessage actuator_hs_state[AGX_MAX_ACTUATOR_NUM];  // v2 only
   ActuatorLSStateMessage actuator_ls_state[AGX_MAX_ACTUATOR_NUM];  // v2 only
   ActuatorStateMessageV1 actuator_state[AGX_MAX_ACTUATOR_NUM];     // v1 only

include/ugv_sdk/details/interface/scout_interface.hpp

@@ -13,9 +13,12 @@
 #include <string>
 
 #include "ugv_sdk/details/interface/agilex_message.h"
+#include "ugv_sdk/details/interface/robot_common_interface.hpp"
 
 namespace westonrobot {
 struct ScoutCoreState {
+  AgxMsgTimeStamp time_stamp;
+
   SystemStateMessage system_state;
   MotionStateMessage motion_state;
   LightStateMessage light_state;
@@ -23,6 +26,8 @@ struct ScoutCoreState {
 };
 
 struct ScoutActuatorState {
+  AgxMsgTimeStamp time_stamp;
+
   // actuator state
   // - for v2 robots only
   ActuatorHSStateMessage actuator_hs_state[4];

include/ugv_sdk/details/interface/tracer_interface.hpp

@@ -13,9 +13,12 @@
 #include <string>
 
 #include "ugv_sdk/details/interface/agilex_message.h"
+#include "ugv_sdk/details/interface/robot_common_interface.hpp"
 
 namespace westonrobot {
 struct TracerCoreState {
+  AgxMsgTimeStamp time_stamp;
+
   SystemStateMessage system_state;
   MotionStateMessage motion_state;
   LightStateMessage light_state;
@@ -23,6 +26,8 @@ struct TracerCoreState {
 };
 
 struct TracerActuatorState {
+  AgxMsgTimeStamp time_stamp;
+
   // actuator state
   ActuatorHSStateMessage actuator_hs_state[2];
   ActuatorLSStateMessage actuator_ls_state[2];

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -22,7 +22,6 @@
 #include <mutex>
 #include <atomic>
 
-#include "ugv_sdk/details/stopwatch.hpp"
 #include "ugv_sdk/details/async_port/async_can.hpp"
 #include "ugv_sdk/details/interface/robot_common_interface.hpp"
 
@@ -79,8 +78,9 @@ class AgilexBase : public RobotCommonInterface {
         msg.body.motion_command_msg.steering_angle = steering_angle;
       }
 
-      std::cout << "sending motion cmd: " << linear_vel << "," << angular_vel
+      //   std::cout << "sending motion cmd: " << linear_vel << "," <<
-                << std::endl;
+      //   angular_vel
+      //             << std::endl;
 
       // send to can bus
       can_frame frame;
@@ -195,27 +195,32 @@ class AgilexBase : public RobotCommonInterface {
     switch (status_msg.type) {
       case AgxMsgSystemState: {
         //   std::cout << "system status feedback received" << std::endl;
+        core_state_msgs_.time_stamp = AgxMsgRefClock::now();
         core_state_msgs_.system_state = status_msg.body.system_state_msg;
         break;
       }
       case AgxMsgMotionState: {
         // std::cout << "motion control feedback received" << std::endl;
+        core_state_msgs_.time_stamp = AgxMsgRefClock::now();
         core_state_msgs_.motion_state = status_msg.body.motion_state_msg;
         break;
       }
       case AgxMsgLightState: {
         // std::cout << "light control feedback received" << std::endl;
+        core_state_msgs_.time_stamp = AgxMsgRefClock::now();
         core_state_msgs_.light_state = status_msg.body.light_state_msg;
         break;
       }
       case AgxMsgMotionModeState: {
         // std::cout << "motion mode feedback received" << std::endl;
+        core_state_msgs_.time_stamp = AgxMsgRefClock::now();
         core_state_msgs_.motion_mode_state =
             status_msg.body.motion_mode_state_msg;
         break;
       }
       case AgxMsgRcState: {
         // std::cout << "rc feedback received" << std::endl;
+        core_state_msgs_.time_stamp = AgxMsgRefClock::now();
         core_state_msgs_.rc_state = status_msg.body.rc_state_msg;
         break;
       }
@@ -229,6 +234,7 @@ class AgilexBase : public RobotCommonInterface {
     switch (status_msg.type) {
       case AgxMsgActuatorHSState: {
         // std::cout << "actuator hs feedback received" << std::endl;
+        actuator_state_msgs_.time_stamp = AgxMsgRefClock::now();
         actuator_state_msgs_
             .actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] =
             status_msg.body.actuator_hs_state_msg;
@@ -236,7 +242,7 @@ class AgilexBase : public RobotCommonInterface {
       }
       case AgxMsgActuatorLSState: {
         // std::cout << "actuator ls feedback received" << std::endl;
-
+        actuator_state_msgs_.time_stamp = AgxMsgRefClock::now();
         actuator_state_msgs_
             .actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
             status_msg.body.actuator_ls_state_msg;
@@ -244,6 +250,7 @@ class AgilexBase : public RobotCommonInterface {
       }
       case AgxMsgActuatorStateV1: {
         // std::cout << "actuator v1 feedback received" << std::endl;
+        actuator_state_msgs_.time_stamp = AgxMsgRefClock::now();
         actuator_state_msgs_
             .actuator_state[status_msg.body.v1_actuator_state_msg.motor_id] =
             status_msg.body.v1_actuator_state_msg;

include/ugv_sdk/details/robot_base/bunker_base.hpp

@@ -41,6 +41,7 @@ class BunkerBase : public AgilexBase<ParserType>, public BunkerInterface {
     auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
 
     BunkerCoreState bunker_state;
+    bunker_state.time_stamp = state.time_stamp;
     bunker_state.system_state = state.system_state;
     bunker_state.motion_state = state.motion_state;
     bunker_state.rc_state = state.rc_state;

include/ugv_sdk/details/robot_base/hunter_base.hpp

@@ -15,12 +15,12 @@
 #include <thread>
 #include <mutex>
 
-#include "ugv_sdk/details/interface/scout_interface.hpp"
+#include "ugv_sdk/details/interface/hunter_interface.hpp"
 #include "ugv_sdk/details/robot_base/agilex_base.hpp"
 
 namespace westonrobot {
 template <typename ParserType>
-class HunterBase : public AgilexBase<ParserType>, public ScoutInterface {
+class HunterBase : public AgilexBase<ParserType>, public HunterInterface {
  public:
   HunterBase() : AgilexBase<ParserType>(){};
   ~HunterBase() = default;
@@ -36,33 +36,29 @@ class HunterBase : public AgilexBase<ParserType>, public ScoutInterface {
                                               angular_vel);
   }
 
-  void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
-                       uint8_t r_value) override {
-    AgilexBase<ParserType>::SendLightCommand(f_mode, f_value, r_mode, r_value);
-  }
-
   // get robot state
-  ScoutCoreState GetRobotState() override {
+  HunterCoreState GetRobotState() override {
     auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
 
-    ScoutCoreState scout_state;
+    HunterCoreState hunter_state;
-    scout_state.system_state = state.system_state;
+    hunter_state.time_stamp = state.time_stamp;
-    scout_state.motion_state = state.motion_state;
+    hunter_state.system_state = state.system_state;
-    scout_state.light_state = state.light_state;
+    hunter_state.motion_state = state.motion_state;
-    scout_state.rc_state = state.rc_state;
+    hunter_state.rc_state = state.rc_state;
-    return scout_state;
+    return hunter_state;
   }
 
-  ScoutActuatorState GetActuatorState() override {
+  HunterActuatorState GetActuatorState() override {
     auto actuator = AgilexBase<ParserType>::GetActuatorStateMsgGroup();
 
-    ScoutActuatorState scout_actuator;
+    HunterActuatorState hunter_actuator;
-    for (int i = 0; i < 4; ++i) {
+    hunter_actuator.time_stamp = actuator.time_stamp;
-      scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
+    for (int i = 0; i < 3; ++i) {
-      scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];
+      hunter_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
-      scout_actuator.actuator_state[i] = actuator.actuator_state[i];
+      hunter_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];
+      hunter_actuator.actuator_state[i] = actuator.actuator_state[i];
     }
-    return scout_actuator;
+    return hunter_actuator;
   }
 };
 }  // namespace westonrobot

include/ugv_sdk/details/robot_base/ranger_base.hpp

@@ -55,6 +55,7 @@ class RangerBase : public AgilexBase<ProtocolV2Parser>, public RangerInterface {
     auto state = AgilexBase<ProtocolV2Parser>::GetRobotCoreStateMsgGroup();
 
     RangerCoreState ranger_state;
+    ranger_state.time_stamp = state.time_stamp;
     ranger_state.system_state = state.system_state;
     ranger_state.motion_state = state.motion_state;
     ranger_state.light_state = state.light_state;
@@ -66,6 +67,7 @@ class RangerBase : public AgilexBase<ProtocolV2Parser>, public RangerInterface {
     auto actuator = AgilexBase<ProtocolV2Parser>::GetActuatorStateMsgGroup();
 
     RangerActuatorState ranger_actuator;
+    ranger_actuator.time_stamp = actuator.time_stamp;
     for (int i = 0; i < 8; ++i) {
       ranger_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
       ranger_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];

include/ugv_sdk/details/robot_base/scout_base.hpp

@@ -50,6 +50,7 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
     auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
 
     ScoutCoreState scout_state;
+    scout_state.time_stamp = state.time_stamp;
     scout_state.system_state = state.system_state;
     scout_state.motion_state = state.motion_state;
     scout_state.light_state = state.light_state;
@@ -61,6 +62,7 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
     auto actuator = AgilexBase<ParserType>::GetActuatorStateMsgGroup();
 
     ScoutActuatorState scout_actuator;
+    scout_actuator.time_stamp = actuator.time_stamp;
     for (int i = 0; i < 4; ++i) {
       scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
       scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];

include/ugv_sdk/details/robot_base/tracer_base.hpp

@@ -31,7 +31,7 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>,
   void Connect(std::string can_name) override {
     AgilexBase<ProtocolV2Parser>::Connect(can_name);
   }
-  
+
   void Connect(std::string uart_name, uint32_t baudrate) override {
     // TODO
   }
@@ -52,6 +52,7 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>,
     auto state = AgilexBase<ProtocolV2Parser>::GetRobotCoreStateMsgGroup();
 
     TracerCoreState tracer_state;
+    tracer_state.time_stamp = state.time_stamp;
     tracer_state.system_state = state.system_state;
     tracer_state.motion_state = state.motion_state;
     tracer_state.light_state = state.light_state;
@@ -63,6 +64,7 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>,
     auto actuator = AgilexBase<ProtocolV2Parser>::GetActuatorStateMsgGroup();
 
     TracerActuatorState tracer_actuator;
+    tracer_actuator.time_stamp = actuator.time_stamp;
     for (int i = 0; i < 2; ++i) {
       tracer_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
       tracer_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];

include/ugv_sdk/utilities/protocol_detector.hpp

@@ -0,0 +1,35 @@
+/*
+ * protocol_detector.hpp
+ *
+ * Created on: Jul 15, 2021 14:03
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef PROTOCOL_DETECTOR_HPP
+#define PROTOCOL_DETECTOR_HPP
+
+#include <atomic>
+
+#include "ugv_sdk/details/async_port/async_can.hpp"
+#include "ugv_sdk/details/interface/parser_interface.hpp"
+
+namespace westonrobot {
+class ProtocolDectctor {
+ public:
+  void Connect(std::string can_name);
+  void Connect(std::string uart_name, uint32_t baudrate);
+
+  ProtocolVersion DetectProtocolVersion(uint32_t timeout_sec);
+
+ private:
+  std::shared_ptr<AsyncCAN> can_;
+  void ParseCANFrame(can_frame *rx_frame);
+
+  std::atomic<bool> msg_v1_detected_;
+  std::atomic<bool> msg_v2_detected_;
+};
+}  // namespace westonrobot
+
+#endif /* PROTOCOL_DETECTOR_HPP */

include/ugv_sdk/utilities/stopwatch.hpp

@@ -9,7 +9,7 @@
  * [2] https://github.com/rxdu/stopwatch
  *
  * Copyright (c) 2019 sailormoon <http://unlicense.org>
- * Copyright (c) 2020 Ruixiang Du (rdu)
+ * Copyright (c) 2020 Weston Robot Pte. Ltd.
  *
  * License: <http://unlicense.org>
  */

src/protocol_v1/agilex_msg_parser_v1.c

@@ -18,7 +18,7 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
   // if checksum not correct
   if (!CalcCanFrameChecksumV1(rx_frame->can_id, (uint8_t *)rx_frame->data,
                               rx_frame->can_dlc)) {
-    printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id);
+    // printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id);
     return false;
   }
 

src/protocol_v2/agilex_msg_parser_v2.c

@@ -182,11 +182,11 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
           ((uint16_t)(frame->driver_voltage.low_byte) |
            (uint16_t)(frame->driver_voltage.high_byte) << 8) *
           0.1;
-      msg->body.actuator_ls_state_msg.driver_temperature =
+      msg->body.actuator_ls_state_msg.driver_temp =
-          (int16_t)((uint16_t)(frame->driver_temperature.low_byte) |
+          (int16_t)((uint16_t)(frame->driver_temp.low_byte) |
-                    (uint16_t)(frame->driver_temperature.high_byte) << 8);
+                    (uint16_t)(frame->driver_temp.high_byte) << 8);
-      msg->body.actuator_ls_state_msg.motor_temperature =
+      msg->body.actuator_ls_state_msg.motor_temp =
-          frame->motor_temperature;
+          frame->motor_temp;
       msg->body.actuator_ls_state_msg.driver_state = frame->driver_state;
       break;
     }

src/protocol_v2/agilex_protocol_v2.h

@@ -242,8 +242,8 @@ typedef struct {
 
 typedef struct {
   struct16_t driver_voltage;
-  struct16_t driver_temperature;
+  struct16_t driver_temp;
-  int8_t motor_temperature;
+  int8_t motor_temp;
   uint8_t driver_state;
   uint8_t reserved0;
   uint8_t reserved1;

src/utilities/protocol_detector.cpp

@@ -0,0 +1,55 @@
+/*
+ * protocol_detector.cpp
+ *
+ * Created on: Jul 15, 2021 14:05
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#include "ugv_sdk/utilities/protocol_detector.hpp"
+#include "ugv_sdk/utilities/stopwatch.hpp"
+
+namespace westonrobot {
+void ProtocolDectctor::Connect(std::string can_name) {
+  can_ = std::make_shared<AsyncCAN>(can_name);
+  can_->SetReceiveCallback(
+      std::bind(&ProtocolDectctor::ParseCANFrame, this, std::placeholders::_1));
+  can_->StartListening();
+}
+void ProtocolDectctor::Connect(std::string uart_name, uint32_t baudrate) {}
+
+ProtocolVersion ProtocolDectctor::DetectProtocolVersion(uint32_t timeout_sec) {
+  msg_v1_detected_ = false;
+  msg_v2_detected_ = false;
+
+  StopWatch sw;
+  Timer timer;
+  while (sw.stoc() < timeout_sec) {
+    timer.reset();
+    if (msg_v1_detected_ || msg_v2_detected_) break;
+    timer.sleep_until_ms(50);
+  }
+
+  // make sure only one version is detected
+  if (msg_v1_detected_ && msg_v2_detected_) return ProtocolVersion::UNKONWN;
+
+  if (msg_v1_detected_)
+    return ProtocolVersion::AGX_V1;
+  else if (msg_v2_detected_)
+    return ProtocolVersion::AGX_V2;
+
+  return ProtocolVersion::UNKONWN;
+};
+
+void ProtocolDectctor::ParseCANFrame(can_frame *rx_frame) {
+  // state feedback frame with id 0x151 is unique to V1 protocol
+  if (rx_frame->can_id == 0x151) {
+    msg_v1_detected_ = true;
+  }
+  // rc state frame with id 0x241 is unique to V2 protocol
+  else if (rx_frame->can_id == 0x241) {
+    msg_v2_detected_ = true;
+  }
+}
+}  // namespace westonrobot