saved work, updated code organization

2023-04-08 06:32:14 +08:00 · 2021-07-09 22:19:21 +08:00
parent 0565639c8f
commit 41f598c65c
20 changed files with 253 additions and 1389 deletions
--- a/include/ugv_sdk/details/interface/agilex_message.h
+++ b/include/ugv_sdk/details/interface/agilex_message.h
@@ -19,9 +19,10 @@ extern "C" {
 #include <stdint.h>
 #include <string.h>

+#include "ugv_sdk/details/interface/agilex_types.h"
+
 /***************** Control messages *****************/

-// 0x111
 typedef struct {
  float linear_velocity;
  float angular_velocity;
@@ -29,51 +30,22 @@ typedef struct {
  float steering_angle;
 } MotionCommandMessage;

-// 0x121
-typedef enum {
-  CONST_OFF = 0x00,
-  CONST_ON = 0x01,
-  BREATH = 0x02,
-  CUSTOM = 0x03
-} LightMode;
-
-typedef struct {
-  LightMode mode;
-  uint8_t custom_value;
-} LightOperation;
-
 typedef struct {
  bool enable_cmd_ctrl;
  LightOperation front_light;
  LightOperation rear_light;
 } LightCommandMessage;

-// 0x131
 typedef struct {
  bool enable_braking;
 } BrakingCommandMessage;

-// 0x141
 typedef struct {
  uint8_t motion_mode;
 } MotionModeMessage;

 /**************** Feedback messages *****************/

-// 0x211
-typedef enum {
-  VehicleStateNormal = 0x00,
-  VehicleStateEStop = 0x01,
-  VehicleStateException = 0x02
-} VehicleState;
-
-typedef enum {
-  //   CONTROL_MODE_STANDBY = 0x00,
-  CONTROL_MODE_RC = 0x00,
-  CONTROL_MODE_CAN = 0x01,
-  CONTROL_MODE_UART = 0x02
-} ControlMode;
-
 #define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
 #define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200)
 #define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001)
@@ -103,13 +75,6 @@ typedef struct {
 // 0x231
 typedef LightCommandMessage LightStateMessage;

-// 0x241
-typedef enum {
-  RC_SWITCH_UP = 0,
-  RC_SWITCH_MIDDLE,
-  RC_SWITCH_DOWN
-} RcSwitchState;
-
 typedef struct {
  RcSwitchState swa;
  RcSwitchState swb;
@@ -122,7 +87,6 @@ typedef struct {
  int8_t var_a;
 } RcStateMessage;

-// 0x251 - 0x258
 typedef struct {
  uint8_t motor_id;
  int16_t rpm;
@@ -130,6 +94,11 @@ typedef struct {
  int32_t pulse_count;
 } ActuatorHSStateMessage;

+typedef struct {
+  uint8_t motion_mode;
+  uint8_t mode_changing;
+} MotionModeFeedbackMessage;
+
 // 0x261 - 0x264
 #define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01)
 #define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02)
@@ -140,12 +109,6 @@ typedef struct {
 #define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40)
 #define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80)

-// 0x291
-typedef struct {
-  uint8_t motion_mode;
-  uint8_t mode_changing;
-} MotionModeFeedbackMessage;
-
 typedef struct {
  uint8_t motor_id;
  float driver_voltage;
@@ -156,45 +119,38 @@ typedef struct {

 /***************** Sensor messages ******************/

-// 0x311
 typedef struct {
  float left_wheel;
  float right_wheel;
 } OdometryMessage;

-// 0x321
 typedef struct {
  float accel_x;
  float accel_y;
  float accel_z;
 } ImuAccelMessage;

-// 0x322
 typedef struct {
  float gyro_x;
  float gyro_y;
  float gyro_z;
 } ImuGyroMessage;

-// 0x323
 typedef struct {
  float yaw;
  float pitch;
  float roll;
 } ImuEulerMessage;

-// 0x331
 typedef struct {
  uint8_t trigger_state;
 } SafetyBumperMessage;

-// 0x340 + num
 typedef struct {
  uint8_t sensor_id;
  uint8_t distance[8];
 } UltrasonicMessage;

-// 0x350 + num
 typedef struct {
  uint8_t sensor_id;
  float relative_distance;
@@ -203,7 +159,6 @@ typedef struct {
  int8_t channels[3];
 } UwbMessage;

-// 0x361
 typedef struct {
  uint8_t battery_soc;
  uint8_t battery_soh;
@@ -250,12 +205,10 @@ typedef struct {

 /************  Query/config messages ****************/

-// 0x411
 typedef struct {
  bool request;
 } VersionRequestMessage;

-// 0x41a
 typedef struct {
  uint16_t controller_hw_version;
  uint16_t motor_driver_hw_version;
@@ -263,22 +216,18 @@ typedef struct {
  uint16_t motor_driver_sw_version;
 } VersionResponseMessage;

-// 0x421
 typedef struct {
  ControlMode mode;
 } ControlModeConfigMessage;

-// 0x431
 typedef struct {
  bool set_as_neutral;
 } SteerNeutralRequestMessage;

-// 0x43a
 typedef struct {
  bool neutral_set_successful;
 } SteerNeutralResponseMessage;

-// 0x441
 typedef enum {
  CLEAR_ALL_FAULT = 0x00,
  CLEAR_MOTOR1_FAULT = 0x01,
--- a/include/ugv_sdk/details/interface/agilex_types.h
+++ b/include/ugv_sdk/details/interface/agilex_types.h
@@ -0,0 +1,54 @@
+/*
+ * agilex_types.h
+ *
+ * Created on: Jul 09, 2021 21:57
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef AGILEX_TYPES_H
+#define AGILEX_TYPES_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+typedef enum {
+  CONST_OFF = 0x00,
+  CONST_ON = 0x01,
+  BREATH = 0x02,
+  CUSTOM = 0x03
+} LightMode;
+
+typedef struct {
+  LightMode mode;
+  uint8_t custom_value;
+} LightOperation;
+
+typedef enum {
+  VehicleStateNormal = 0x00,
+  VehicleStateEStop = 0x01,
+  VehicleStateException = 0x02
+} VehicleState;
+
+typedef enum {
+  //   CONTROL_MODE_STANDBY = 0x00,
+  CONTROL_MODE_RC = 0x00,
+  CONTROL_MODE_CAN = 0x01,
+  CONTROL_MODE_UART = 0x02
+} ControlMode;
+
+typedef enum {
+  RC_SWITCH_UP = 0,
+  RC_SWITCH_MIDDLE,
+  RC_SWITCH_DOWN
+} RcSwitchState;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* AGILEX_TYPES_H */
--- a/include/ugv_sdk/details/protocol_v1/agilex_message_v1.h
+++ b/include/ugv_sdk/details/protocol_v1/agilex_message_v1.h
@@ -1,167 +0,0 @@
-/*
- * agilex_message_v1.h
- *
- * Created on: Jul 09, 2021 20:55
- * Description:
- *  all values are using SI units (e.g. meter/second/radian)
- *
- * Re-implemented as a subset of AgxMessage defined in protocol v2
- *
- * Copyright (c) 2021 Ruixiang Du (rdu)
- */
-
-#ifndef AGILEX_MESSAGE_V1_H
-#define AGILEX_MESSAGE_V1_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <string.h>
-
-/***************** Control messages *****************/
-
-typedef enum {
-  //   CONTROL_MODE_STANDBY = 0x00,
-  CONTROL_MODE_RC = 0x00,
-  CONTROL_MODE_CAN = 0x01,
-  CONTROL_MODE_UART = 0x02
-} ControlMode;
-
-// 0x130
-typedef struct {
-  ControlMode control_mode;
-  uint8_t fault_clear;
-
-  float linear;
-  float angular;
-} MotionCommandMessage;
-
-// 0x140
-typedef enum {
-  CONST_OFF = 0x00,
-  CONST_ON = 0x01,
-  BREATH = 0x02,
-  CUSTOM = 0x03
-} LightMode;
-
-typedef struct {
-  LightMode mode;
-  uint8_t custom_value;
-} LightOperation;
-
-typedef struct {
-  bool enable_cmd_ctrl;
-  LightOperation front_light;
-  LightOperation rear_light;
-} LightCommandMessage;
-
-typedef struct {
-  bool set_neutral;
-} ValueSetCommandMessage;
-
-/**************** Feedback messages *****************/
-
-typedef enum {
-  VehicleStateNormal = 0x00,
-  VehicleStateEStop = 0x01,
-  VehicleStateException = 0x02
-} VehicleState;
-
-#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
-#define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200)
-#define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001)
-#define SYSTEM_ERROR_BATTERY_WARNING_MASK ((uint16_t)0x0002)
-#define SYSTEM_ERROR_RC_SIGNAL_LOSS_MASK ((uint16_t)0x0004)
-#define SYSTEM_ERROR_MOTOR1_COMM_MASK ((uint16_t)0x0008)
-#define SYSTEM_ERROR_MOTOR2_COMM_MASK ((uint16_t)0x0010)
-#define SYSTEM_ERROR_MOTOR3_COMM_MASK ((uint16_t)0x0020)
-#define SYSTEM_ERROR_MOTOR4_COMM_MASK ((uint16_t)0x0040)
-#define SYSTEM_ERROR_STEER_ENCODER_MASK ((uint16_t)0x0080)
-
-typedef struct {
-  VehicleState vehicle_state;
-  ControlMode control_mode;
-  float battery_voltage;
-  uint16_t error_code;
-} SystemStateMessage;
-
-// 0x221
-typedef struct {
-  float linear_velocity;
-  float angular_velocity;  // only valid for differential drivering
-  float lateral_velocity;
-  float steering_angle;  // only valid for ackermann steering
-} MotionStateMessage;
-
-// 0x231
-typedef LightCommandMessage LightStateMessage;
-
-// 0x251 - 0x258
-typedef struct {
-  uint8_t motor_id;
-  int16_t rpm;
-  float current;
-  int32_t pulse_count;
-} ActuatorStateMessage;
-
-// 0x261 - 0x264
-#define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01)
-#define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02)
-#define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04)
-#define DRIVER_STATE_DRIVER_OVERHEAT_MASK ((uint8_t)0x08)
-#define DRIVER_STATE_SENSOR_FAULT_MASK ((uint8_t)0x10)
-#define DRIVER_STATE_DRIVER_FAULT_MASK ((uint8_t)0x20)
-#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40)
-#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80)
-
-// 0x291
-typedef struct {
-  uint8_t motion_mode;
-  uint8_t mode_changing;
-} MotionModeFeedbackMessage;
-
-typedef struct {
-  uint8_t motor_id;
-  float driver_voltage;
-  float driver_temperature;
-  int8_t motor_temperature;
-  uint8_t driver_state;
-} ActuatorLSStateMessage;
-
-//////////////////////////////////////////////////////
-
-typedef enum {
-  AgxMsgUnkonwn = 0x00,
-  // command
-  AgxMsgMotionCommand,
-  AgxMsgLightCommand,
-  AgxMsgValueSetCommand,
-  // state feedback
-  AgxMsgSystemState,
-  AgxMsgMotionState,
-  AgxMsgLightState,
-  AgxMsgActuatorState,
-} MsgType;
-
-typedef struct {
-  MsgType type;
-  union {
-    // command
-    MotionCommandMessage motion_command_msg;
-    LightCommandMessage light_command_msg;
-    // state feedback
-    SystemStateMessage system_state_msg;
-    MotionStateMessage motion_state_msg;
-    LightStateMessage light_state_msg;
-    ActuatorStateMessage actuator_state_msg;
-  } body;
-} AgxMessageV1;
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* AGILEX_MESSAGE_V1_H */
--- a/include/ugv_sdk/details/robot_base/ranger_base.cpp
+++ b/include/ugv_sdk/details/robot_base/ranger_base.cpp
@@ -0,0 +1,111 @@
+/**
+ * @Kit       : Qt-Creator: Desktop
+ * @Author    : Wang Zhe
+ * @Date      : 2021-04-19  19:36:24
+ * @FileName  : ranger_base.cpp
+ * @Mail      : wangzheqie@qq.com
+ * Copyright  : AgileX Robotics
+ **/
+
+#include "ugv_sdk/protocol_v2/ranger_base.hpp"
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <ratio>
+#include <string>
+#include <thread>
+
+#include "stopwatch.hpp"
+
+#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
+
+namespace westonrobot {
+void RangerBase::Connect(std::string dev_name) {
+  AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame, this,
+                                          std::placeholders::_1));
+}
+
+void RangerBase::SetMotionCommand(double linear_vel, double steer_angle,
+                                  double lateral_vel, double angular_vel) {
+  AgilexBase::SetMotionCommand(linear_vel, angular_vel, lateral_vel,
+                               steer_angle / 10.0);
+}
+
+void RangerBase::SetLightCommand(const RangerLightCmd &cmd) {
+  if (cmd.enable_cmd_ctrl) {
+    AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
+                                 LightMode::CONST_OFF, 0);
+  }
+}
+
+void RangerBase::SetMotionMode(uint8_t mode) {
+  AgilexBase::SetMotionMode(mode);
+}
+
+RangerState RangerBase::GetRangerState() {
+  std::lock_guard<std::mutex> guard(state_mutex_);
+  return ranger_state_;
+}
+
+void RangerBase::ParseCANFrame(can_frame *rx_frame) {
+  AgxMessage status_msg;
+  DecodeCanFrame(rx_frame, &status_msg);
+  std::lock_guard<std::mutex> guard(state_mutex_);
+  UpdateRangerState(status_msg, ranger_state_);
+}
+
+void RangerBase::UpdateRangerState(const AgxMessage &status_msg,
+                                   RangerState &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;
+      state.motion_state.steering_angle *= 10;
+      // std::cout << "steering angle: " << state.motion_state.steering_angle <<
+      // std::endl;
+      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;
+    }
+    case AgxMsgMotionModeState: {
+      state.current_motion_mode = status_msg.body.motion_mode_feedback_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
--- a/include/ugv_sdk/details/robot_base/tracer_base.cpp
+++ b/include/ugv_sdk/details/robot_base/tracer_base.cpp
@@ -0,0 +1,91 @@
+#include "ugv_sdk/protocol_v2/tracer_base.hpp"
+
+#include <string>
+#include <cstring>
+#include <iostream>
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ratio>
+#include <thread>
+
+#include "stopwatch.hpp"
+
+#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
+
+namespace westonrobot {
+void TracerBaseV2::Connect(std::string dev_name) {
+  AgilexBase::ConnectPort(dev_name, std::bind(&TracerBaseV2::ParseCANFrame, this,
+                                              std::placeholders::_1));
+}
+
+void TracerBaseV2::Connect(std::string uart_name, uint32_t baudrate) {
+  // TODO
+}
+
+void TracerBaseV2::SetMotionCommand(double linear_vel, double angular_vel) {
+  AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
+}
+
+void TracerBaseV2::SetLightCommand(const TracerLightCmd &cmd) {
+  if (cmd.enable_cmd_ctrl) {
+    AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
+                                 LightMode::CONST_OFF, 0);
+  }
+}
+
+TracerState TracerBaseV2::GetTracerState() {
+  std::lock_guard<std::mutex> guard(state_mutex_);
+  return tracer_state_;
+}
+
+void TracerBaseV2::ParseCANFrame(can_frame *rx_frame) {
+  AgxMessage status_msg;
+  DecodeCanFrame(rx_frame, &status_msg);
+  std::lock_guard<std::mutex> guard(state_mutex_);
+  UpdateTracerState(status_msg, tracer_state_);
+}
+
+void TracerBaseV2::UpdateTracerState(const AgxMessage &status_msg,
+                                   TracerState &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;
+    }
+  }
+}
+}  // namespace westonrobot