Merge pull request #10 from chilatiao/next

add brakeconfig function

include/ugv_sdk/details/interface/agilex_message.h

@@ -240,6 +240,10 @@ typedef struct {
   ControlMode mode;
 } ControlModeConfigMessage;
 
+typedef struct {
+  BrakeMode mode;
+} BrakeModeConfigMessage;
+
 typedef struct {
   bool set_as_neutral;
 } SteerNeutralRequestMessage;
@@ -295,6 +299,7 @@ typedef enum {
   AgxMsgSteerNeutralRequest,
   AgxMsgSteerNeutralResponse,
   AgxMsgStateResetConfig,
+  AgxMsgBrakeModeConfig,
   // V1-only messages
   AgxMsgMotionCommandV1,
   AgxMsgValueSetCommandV1,
@@ -333,6 +338,7 @@ typedef struct {
     VersionRequestMessage version_request_msg;
     VersionResponseMessage version_response_msg;
     ControlModeConfigMessage control_mode_config_msg;
+    BrakeModeConfigMessage brake_mode_config_msg;
     SteerNeutralRequestMessage steer_neutral_request_msg;
     SteerNeutralResponseMessage steer_neutral_response_msg;
     StateResetConfigMessage state_reset_config_msg;

include/ugv_sdk/details/interface/agilex_types.h

@@ -41,6 +41,12 @@ typedef enum {
   CONTROL_MODE_UART = 0x02
 } ControlMode;
 
+typedef enum {
+  //   CONTROL_MODE_STANDBY = 0x00,
+  BRAKE_MODE_UNLOCK = 0x00,
+  BRAKE_MODE_LOCK = 0x01
+} BrakeMode;
+
 typedef enum {
   RC_SWITCH_UP = 0,
   RC_SWITCH_MIDDLE,

include/ugv_sdk/details/interface/hunter_interface.hpp

@@ -39,6 +39,8 @@ struct HunterInterface {
 
   virtual void SetMotionCommand(double linear_vel, double steering_angle) = 0;
 
+  virtual void SetBrakeMode(BrakeMode mode) = 0;
+
   // get robot state
   virtual HunterCoreState GetRobotState() = 0;
   virtual HunterActuatorState GetActuatorState() = 0;

include/ugv_sdk/details/interface/robot_common_interface.hpp

@@ -93,6 +93,8 @@ class RobotCommonInterface {
         "Only a derived version of this function with actual implementation "
         "is supposed to be used.");
   };
+
+  virtual void EnableBrakeMode(BrakeMode mode){};
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -268,6 +268,17 @@ class AgilexBase : public RobotCommonInterface {
         break;
     }
   }
+
+  void EnableBrakedMode(BrakeMode mode) {
+    // construct message
+    AgxMessage msg;
+    msg.type = AgxMsgBrakeModeConfig;
+    msg.body.brake_mode_config_msg.mode = mode;
+
+    // encode msg to can frame and send to bus
+    can_frame frame;
+    if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);
+  }
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/robot_base/hunter_base.hpp

@@ -36,6 +36,7 @@ class HunterBase : public AgilexBase<ParserType>, public HunterInterface {
                                               angular_vel);
   }
 
+
   // get robot state
   HunterCoreState GetRobotState() override {
     auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
@@ -60,6 +61,10 @@ class HunterBase : public AgilexBase<ParserType>, public HunterInterface {
     }
     return hunter_actuator;
   }
+
+  void SetBrakeMode(BrakeMode mode) override{
+    AgilexBase<ParserType>::EnableBrakeMode(mode);
+  }
 };
 }  // namespace westonrobot
 

include/ugv_sdk/mobile_robot/hunter_robot.hpp

@@ -25,6 +25,8 @@ class HunterRobot : public RobotCommonInterface, public HunterInterface {
 
   void EnableCommandedMode() override;
 
+  void SetBrakeMode(BrakeMode mode) override;
+
   void SetMotionCommand(double linear_vel, double angular_vel) override;
 
   void ResetRobotState() override;

src/mobile_robot/hunter_robot.cpp

@@ -38,6 +38,11 @@ void HunterRobot::SetMotionCommand(double linear_vel, double angular_vel) {
   hunter->SetMotionCommand(linear_vel, angular_vel);
 }
 
+void HunterRobot::SetBrakeMode(BrakeMode mode) {
+  auto hunter = dynamic_cast<HunterInterface*>(robot_);
+  hunter->SetBrakeMode(mode);
+}
+
 HunterCoreState HunterRobot::GetRobotState() {
   auto hunter = dynamic_cast<HunterInterface*>(robot_);
   return hunter->GetRobotState();

src/protocol_v2/agilex_msg_parser_v2.c

@@ -563,6 +563,21 @@ bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
       memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
       break;
     }
+    case AgxMsgBrakeModeConfig: {
+      tx_frame->can_id = CAN_MSG_BRAKING_COMMAND_ID;
+      tx_frame->can_dlc = 8;
+      BrakeModeConfigFrame frame;
+      frame.mode = msg->body.control_mode_config_msg.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;
+    }
     case AgxMsgSteerNeutralRequest: {
       tx_frame->can_id = CAN_MSG_STEER_NEUTRAL_REQUEST_ID;
       tx_frame->can_dlc = 8;

src/protocol_v2/agilex_protocol_v2.h

@@ -360,6 +360,17 @@ typedef struct {
   uint8_t reserved6;
 } ControlModeConfigFrame;
 
+typedef struct {
+  uint8_t mode;
+  uint8_t reserved0;
+  uint8_t reserved1;
+  uint8_t reserved2;
+  uint8_t reserved3;
+  uint8_t reserved4;
+  uint8_t reserved5;
+  uint8_t reserved6;
+} BrakeModeConfigFrame;
+
 typedef struct {
   uint8_t set_as_neutral;
   uint8_t reserved0;