saved work

src/protocol_v2/scout_base.cpp

@@ -0,0 +1,98 @@
+/*
+ * scout_base.cpp
+ *
+ * Created on: Jul 08, 2021 12:07
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#include "ugv_sdk/protocol_v2/scout_base_v2.hpp"
+
+#include <string>
+#include <cstring>
+#include <iostream>
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ratio>
+#include <thread>
+
+#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
+
+namespace westonrobot {
+void ScoutBase::Connect(std::string dev_name) {
+  AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBase::ParseCANFrame, this,
+                                              std::placeholders::_1));
+}
+
+void ScoutBase::Connect(std::string uart_name, uint32_t baudrate) {
+  // TODO
+}
+
+void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) {
+  AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
+}
+
+void ScoutBase::SetLightCommand(LightMode f_mode, uint8_t f_value,
+                                LightMode r_mode, uint8_t r_value) {
+  AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value);
+}
+
+ScoutState ScoutBase::GetRobotState() {
+  std::lock_guard<std::mutex> guard(state_mutex_);
+  return scout_state_;
+}
+
+void ScoutBase::ParseCANFrame(can_frame *rx_frame) {
+  AgxMessage status_msg;
+  DecodeCanFrame(rx_frame, &status_msg);
+  std::lock_guard<std::mutex> guard(state_mutex_);
+  UpdateScoutState(status_msg, scout_state_);
+}
+
+void ScoutBase::UpdateScoutState(const AgxMessage &status_msg,
+                                 ScoutState &state) {
+  switch (status_msg.type) {
+    case AgxMsgSystemState: {
+      //   std::cout << "system status feedback received" << std::endl;
+      state.system_state = status_msg.body.system_state_msg;
+      break;
+    }
+    case AgxMsgMotionState: {
+      // std::cout << "motion control feedback received" << std::endl;
+      state.motion_state = status_msg.body.motion_state_msg;
+      break;
+    }
+    case AgxMsgLightState: {
+      // std::cout << "light control feedback received" << std::endl;
+      state.light_state = status_msg.body.light_state_msg;
+      break;
+    }
+    case AgxMsgRcState: {
+      state.rc_state = status_msg.body.rc_state_msg;
+      break;
+    }
+    case AgxMsgActuatorHSState: {
+      // std::cout << "actuator hs feedback received" << std::endl;
+      state.actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] =
+          status_msg.body.actuator_hs_state_msg;
+      break;
+    }
+    case AgxMsgActuatorLSState: {
+      // std::cout << "actuator ls feedback received" << std::endl;
+      state.actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
+          status_msg.body.actuator_ls_state_msg;
+      break;
+    }
+    /* sensor feedback */
+    case AgxMsgOdometry: {
+      // std::cout << "Odometer msg feedback received" << std::endl;
+      state.odometry = status_msg.body.odometry_msg;
+    }
+    default:
+      break;
+  }
+}
+}  // namespace westonrobot