added hunter support

include/ugv_sdk/details/interface/hunter_interface.hpp

@@ -0,0 +1,44 @@
+/*
+ * hunter_interface.hpp
+ *
+ * Created on: Jul 14, 2021 23:21
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef HUNTER_INTERFACE_HPP
+#define HUNTER_INTERFACE_HPP
+
+#include <string>
+
+#include "ugv_sdk/details/interface/agilex_message.h"
+
+namespace westonrobot {
+struct HunterCoreState {
+  SystemStateMessage system_state;
+  MotionStateMessage motion_state;
+  LightStateMessage light_state;
+  RcStateMessage rc_state;
+};
+
+struct HunterActuatorState {
+  // actuator state
+  ActuatorHSStateMessage actuator_hs_state[3];
+  ActuatorLSStateMessage actuator_ls_state[3];
+  // - for v1 robots only
+  ActuatorStateMessageV1 actuator_state[3];
+};
+
+struct HunterInterface {
+  virtual ~HunterInterface() = default;
+
+  virtual void SetMotionCommand(double linear_vel, double steering_angle) = 0;
+
+  // get robot state
+  virtual HunterCoreState GetRobotState() = 0;
+  virtual HunterActuatorState GetActuatorState() = 0;
+};
+}  // namespace westonrobot
+
+#endif /* HUNTER_INTERFACE_HPP */

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -65,7 +65,10 @@ class AgilexBase : public RobotInterface {
         msg.body.v1_motion_command_msg.control_mode = CONTROL_MODE_CAN;
         msg.body.v1_motion_command_msg.clear_all_error = false;
         msg.body.v1_motion_command_msg.linear = linear_vel;
-        msg.body.v1_motion_command_msg.angular = angular_vel;
+        // normally only one of angular_vel and steering_angle can be non-zero
+        msg.body.v1_motion_command_msg.angular =
+            std::abs(angular_vel) > std::abs(steering_angle) ? angular_vel
+                                                             : steering_angle;
         msg.body.v1_motion_command_msg.lateral = lateral_velocity;
       } else if (parser_.GetProtocolVersion() == ProtocolVersion::AGX_V2) {
         msg.type = AgxMsgMotionCommand;
@@ -206,7 +209,8 @@ class AgilexBase : public RobotInterface {
       }
       case AgxMsgMotionModeState: {
         // std::cout << "motion mode feedback received" << std::endl;
-        core_state_msgs_.motion_mode_state = status_msg.body.motion_mode_state_msg;
+        core_state_msgs_.motion_mode_state =
+            status_msg.body.motion_mode_state_msg;
         break;
       }
       case AgxMsgRcState: {

include/ugv_sdk/details/robot_base/hunter_base.hpp

@@ -0,0 +1,78 @@
+/*
+ * hunter_base.hpp
+ *
+ * Created on: Jul 14, 2021 23:23
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef HUNTER_BASE_HPP
+#define HUNTER_BASE_HPP
+
+#include <string>
+#include <cstdint>
+#include <thread>
+#include <mutex>
+
+#include "ugv_sdk/details/interface/scout_interface.hpp"
+#include "ugv_sdk/details/robot_base/agilex_base.hpp"
+
+namespace westonrobot {
+template <typename ParserType>
+class HunterBase : public AgilexBase<ParserType>, public ScoutInterface {
+ public:
+  HunterBase() : AgilexBase<ParserType>(){};
+  ~HunterBase() = default;
+
+  // set up connection
+  void Connect(std::string can_name) override {
+    AgilexBase<ParserType>::Connect(can_name);
+  }
+
+  // robot control
+  void SetMotionCommand(double linear_vel, double angular_vel) override {
+    AgilexBase<ParserType>::SendMotionCommand(linear_vel, 0.0, 0.0,
+                                              angular_vel);
+  }
+
+  void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
+                       uint8_t r_value) override {
+    AgilexBase<ParserType>::SendLightCommand(f_mode, f_value, r_mode, r_value);
+  }
+
+  // get robot state
+  ScoutCoreState GetRobotState() override {
+    auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
+
+    ScoutCoreState scout_state;
+    scout_state.system_state = state.system_state;
+    scout_state.motion_state = state.motion_state;
+    scout_state.light_state = state.light_state;
+    scout_state.rc_state = state.rc_state;
+    return scout_state;
+  }
+
+  ScoutActuatorState GetActuatorState() override {
+    auto actuator = AgilexBase<ParserType>::GetActuatorStateMsgGroup();
+
+    ScoutActuatorState scout_actuator;
+    for (int i = 0; i < 4; ++i) {
+      scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
+      scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];
+      scout_actuator.actuator_state[i] = actuator.actuator_state[i];
+    }
+    return scout_actuator;
+  }
+};
+}  // namespace westonrobot
+
+#include "ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp"
+#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
+
+namespace westonrobot {
+using HunterBaseV1 = HunterBase<HunterProtocolV1Parser>;
+using HunterBaseV2 = HunterBase<ProtocolV2Parser>;
+}  // namespace westonrobot
+
+#endif /* HUNTER_BASE_HPP */

include/ugv_sdk/mobile_robot/hunter_robot.hpp

@@ -0,0 +1,43 @@
+/*
+ * hunter_robot.hpp
+ *
+ * Created on: Jul 08, 2021 10:59
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#ifndef SCOUT_ROBOT_HPP
+#define SCOUT_ROBOT_HPP
+
+#include <memory>
+
+#include "ugv_sdk/details/interface/robot_interface.hpp"
+#include "ugv_sdk/details/interface/hunter_interface.hpp"
+
+namespace westonrobot {
+class HunterRobot : public RobotInterface, public HunterInterface {
+ public:
+  HunterRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2);
+  ~HunterRobot();
+
+  void Connect(std::string can_name) override;
+
+  void EnableCommandedMode() override;
+
+  void SetMotionCommand(double linear_vel, double angular_vel) override;
+
+  void ResetRobotState() override;
+
+  ProtocolVersion GetProtocolVersion() override;
+
+  // get robot state
+  HunterCoreState GetRobotState() override;
+  HunterActuatorState GetActuatorState() override;
+
+ private:
+  RobotInterface* robot_;
+};
+}  // namespace westonrobot
+
+#endif /* SCOUT_ROBOT_HPP */