add ranger motion mode setting

src/agilex_base.cpp

@@ -152,4 +152,18 @@ void AgilexBase::DisableLightControl() {
   EncodeCanFrame(&msg, &frame);
   can_->SendFrame(frame);
 }
-}  // namespace westonrobot
+
+void AgilexBase::SetMotionMode(uint8_t mode)
+{
+  printf("before set: %d", 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);
+   printf("after set: %d", mode);
+}
+}  // 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;
@@ -352,6 +352,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

@@ -30,7 +30,7 @@ void RangerBase::Connect(std::string dev_name) {
 }
 
 void RangerBase::SetMotionCommand(double linear_vel, double angular_vel) {
-  AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
+  AgilexBase::SetMotionCommand(linear_vel, 0.0, 0.0, angular_vel/10.0);
 }
 
 void RangerBase::SetLightCommand(const RangerLightCmd &cmd) {
@@ -40,6 +40,11 @@ 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_;