add ranger motion mode setting

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
@@ -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

@@ -68,6 +68,7 @@ class RangerBase : public AgilexBase {
   // robot control
   void SetMotionCommand(double linear_vel, double angular_vel);
   void SetLightCommand(const RangerLightCmd &cmd);
+  void SetMotionMode(uint8_t mode);
 
   // get robot state
   RangerState GetRangerState();

src/agilex_base.cpp

@@ -152,4 +152,18 @@ void AgilexBase::DisableLightControl() {
   EncodeCanFrame(&msg, &frame);
   can_->SendFrame(frame);
 }
+
+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_;