Merge pull request #8 from wangzheqie/v2.x

V2.x

include/ugv_sdk/agilex_base.hpp

@@ -59,6 +59,9 @@ class AgilexBase {
                         LightMode rear_mode, uint8_t rear_custom_value);
   void DisableLightControl();
 
+  // motion mode change
+  void SetMotionMode(uint8_t mode);
+
   // reset fault states
   void ResetRobotState();
 

include/ugv_sdk/agilex_message.h

@@ -53,6 +53,12 @@ typedef struct {
   bool enable_braking;
 } BrakingCommandMessage;
 
+// 0x141
+typedef  struct
+{
+  uint8_t motion_mode;
+} MotionModeMessage;
+
 /**************** Feedback messages *****************/
 
 // 0x211
@@ -117,7 +123,7 @@ typedef struct {
   int8_t var_a;
 } RcStateMessage;
 
-// 0x251 - 0x254
+// 0x251 - 0x258
 typedef struct {
   uint8_t motor_id;
   int16_t rpm;
@@ -288,6 +294,7 @@ typedef enum {
   AgxMsgMotionCommand,
   AgxMsgLightCommand,
   AgxMsgBrakingCommand,
+  AgxMsgSetMotionMode,
   // state feedback
   AgxMsgSystemState,
   AgxMsgMotionState,
@@ -321,6 +328,7 @@ typedef struct {
     MotionCommandMessage motion_command_msg;
     LightCommandMessage light_command_msg;
     BrakingCommandMessage braking_command_msg;
+    MotionModeMessage motion_mode_msg;
     // state feedback
     SystemStateMessage system_state_msg;
     MotionStateMessage motion_state_msg;

include/ugv_sdk/ranger_base.hpp

@@ -24,10 +24,12 @@ struct RangerState {
   MotionStateMessage motion_state;
   LightStateMessage light_state;
 
+
+
   RcStateMessage rc_state;
 
-  ActuatorHSStateMessage actuator_hs_state[4];
-  ActuatorLSStateMessage actuator_ls_state[4];
+  ActuatorHSStateMessage actuator_hs_state[8];
+  ActuatorLSStateMessage actuator_ls_state[8];
 
   // sensor data
   OdometryMessage odometry;
@@ -66,8 +68,10 @@ class RangerBase : public AgilexBase {
   void Connect(std::string dev_name) override;
 
   // robot control
-  void SetMotionCommand(double linear_vel, double angular_vel);
+  void SetMotionCommand(double linear_vel, double steer_angle,
+                        double lateral_vel = 0.0, double angular_vel = 0.0);
   void SetLightCommand(const RangerLightCmd &cmd);
+  void SetMotionMode(uint8_t mode);
 
   // get robot state
   RangerState GetRangerState();

src/agilex_base.cpp

@@ -152,4 +152,16 @@ void AgilexBase::DisableLightControl() {
   EncodeCanFrame(&msg, &frame);
   can_->SendFrame(frame);
 }
+
+void AgilexBase::SetMotionMode(uint8_t mode)
+{
+   AgxMessage msg;
+   msg.type = AgxMsgSetMotionMode;
+   msg.body.motion_mode_msg.motion_mode = mode;
+
+   // send to can bus
+   can_frame frame;
+   EncodeCanFrame(&msg, &frame);
+   can_->SendFrame(frame);
+}
 }  // namespace westonrobot

src/agx_msg_parser.c

@@ -7,11 +7,11 @@
  * Copyright (c) 2019 Weston Robot Pte. Ltd.
  */
 
-#include "ugv_sdk/details/agilex_msg_parser.h"
 #include "agx_protocol_v2.h"
+#include "ugv_sdk/details/agilex_msg_parser.h"
 
-#include "string.h"
 #include "stdio.h"
+#include "string.h"
 
 bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
   msg->type = AgxMsgUnkonwn;
@@ -145,7 +145,11 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
     case CAN_MSG_ACTUATOR1_HS_STATE_ID:
     case CAN_MSG_ACTUATOR2_HS_STATE_ID:
     case CAN_MSG_ACTUATOR3_HS_STATE_ID:
-    case CAN_MSG_ACTUATOR4_HS_STATE_ID: {
+    case CAN_MSG_ACTUATOR4_HS_STATE_ID:
+    case CAN_MSG_ACTUATOR5_HS_STATE_ID:
+    case CAN_MSG_ACTUATOR6_HS_STATE_ID:
+    case CAN_MSG_ACTUATOR7_HS_STATE_ID:
+    case CAN_MSG_ACTUATOR8_HS_STATE_ID: {
       msg->type = AgxMsgActuatorHSState;
       ActuatorHSStateFrame *frame = (ActuatorHSStateFrame *)(rx_frame->data);
       msg->body.actuator_hs_state_msg.motor_id =
@@ -167,7 +171,11 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
     case CAN_MSG_ACTUATOR1_LS_STATE_ID:
     case CAN_MSG_ACTUATOR2_LS_STATE_ID:
     case CAN_MSG_ACTUATOR3_LS_STATE_ID:
-    case CAN_MSG_ACTUATOR4_LS_STATE_ID: {
+    case CAN_MSG_ACTUATOR4_LS_STATE_ID:
+    case CAN_MSG_ACTUATOR5_LS_STATE_ID:
+    case CAN_MSG_ACTUATOR6_LS_STATE_ID:
+    case CAN_MSG_ACTUATOR7_LS_STATE_ID:
+    case CAN_MSG_ACTUATOR8_LS_STATE_ID: {
       msg->type = AgxMsgActuatorLSState;
       ActuatorLSStateFrame *frame = (ActuatorLSStateFrame *)(rx_frame->data);
       msg->body.actuator_hs_state_msg.motor_id =
@@ -352,6 +360,22 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
       memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
       break;
     }
+    case AgxMsgSetMotionMode: {
+      tx_frame->can_id = CAN_MSG_SET_MOTION_MODE_ID;
+      tx_frame->can_dlc = 8;
+      SetMotionModeFrame frame;
+      frame.motion_mode = msg->body.motion_mode_msg.motion_mode;
+      frame.reserved0 = 0;
+      frame.reserved1 = 0;
+      frame.reserved2 = 0;
+      frame.reserved3 = 0;
+      frame.reserved4 = 0;
+      frame.reserved5 = 0;
+      frame.reserved6 = 0;
+      memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
+      break;
+    }
+
     /***************** feedback frame ****************/
     case AgxMsgSystemState: {
       tx_frame->can_id = CAN_MSG_SYSTEM_STATE_ID;

src/agx_protocol_v2.h

@@ -42,6 +42,7 @@ extern "C" {
 #define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x111)
 #define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x121)
 #define CAN_MSG_BRAKING_COMMAND_ID ((uint32_t)0x131)
+#define CAN_MSG_SET_MOTION_MODE_ID  ((uint32_t)0x141)
 
 // state feedback group: 0x2
 #define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x211)
@@ -178,6 +179,18 @@ typedef struct {
   uint8_t count;
 } BrakingCommandFrame;
 
+typedef struct
+{
+  uint8_t motion_mode;
+  uint8_t reserved0;
+  uint8_t reserved1;
+  uint8_t reserved2;
+  uint8_t reserved3;
+  uint8_t reserved4;
+  uint8_t reserved5;
+  uint8_t reserved6;
+}SetMotionModeFrame;
+
 // State feedback messages
 typedef struct {
   uint8_t vehicle_state;

src/ranger_base.cpp

@@ -29,8 +29,10 @@ void RangerBase::Connect(std::string dev_name) {
                                           std::placeholders::_1));
 }
 
-void RangerBase::SetMotionCommand(double linear_vel, double angular_vel) {
-  AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
+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) {
@@ -40,6 +42,10 @@ void RangerBase::SetLightCommand(const RangerLightCmd &cmd) {
   }
 }
 
+void RangerBase::SetMotionMode(uint8_t mode) {
+  AgilexBase::SetMotionMode(mode);
+}
+
 RangerState RangerBase::GetRangerState() {
   std::lock_guard<std::mutex> guard(state_mutex_);
   return ranger_state_;