added time stamp to robot states

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;
-  int8_t motor_temperature;
+  float driver_temp;
+  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::endl;
+      //   std::cout << "sending motion cmd: " << linear_vel << "," <<
+      //   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;
-    scout_state.system_state = state.system_state;
-    scout_state.motion_state = state.motion_state;
-    scout_state.light_state = state.light_state;
-    scout_state.rc_state = state.rc_state;
-    return scout_state;
+    HunterCoreState hunter_state;
+    hunter_state.time_stamp = state.time_stamp;
+    hunter_state.system_state = state.system_state;
+    hunter_state.motion_state = state.motion_state;
+    hunter_state.rc_state = state.rc_state;
+    return hunter_state;
   }
 
-  ScoutActuatorState GetActuatorState() override {
+  HunterActuatorState GetActuatorState() override {
     auto actuator = AgilexBase<ParserType>::GetActuatorStateMsgGroup();
 
-    ScoutActuatorState scout_actuator;
-    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];
-      scout_actuator.actuator_state[i] = actuator.actuator_state[i];
+    HunterActuatorState hunter_actuator;
+    hunter_actuator.time_stamp = actuator.time_stamp;
+    for (int i = 0; i < 3; ++i) {
+      hunter_actuator.actuator_hs_state[i] = actuator.actuator_hs_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>
  */