added hunter support

CMakeLists.txt

@@ -88,6 +88,7 @@ add_library(${PROJECT_NAME}
     ########################
     ## public interface to access robot
     src/mobile_robot/scout_robot.cpp
+    src/mobile_robot/hunter_robot.cpp
     src/mobile_robot/bunker_robot.cpp
     ########################
     ## protocol v2 support

demo/CMakeLists.txt

@@ -3,12 +3,4 @@ add_subdirectory(scout_demo)
 add_subdirectory(tracer_demo)
 add_subdirectory(ranger_demo)
 add_subdirectory(bunker_demo)
-
-# add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp)
-# target_link_libraries(app_hunter_demo ugv_sdk)
-
-# add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp)
-# target_link_libraries(app_tracer_demo ugv_sdk)
-
-# add_executable(app_ranger_demo ranger_demo/ranger_demo.cpp)
-# target_link_libraries(app_ranger_demo ugv_sdk)
+add_subdirectory(hunter_demo)

demo/hunter_demo/CMakeLists.txt

@@ -0,0 +1,3 @@
+# tests
+add_executable(demo_hunter_robot hunter_robot_demo.cpp)
+target_link_libraries(demo_hunter_robot ugv_sdk)

demo/hunter_demo/hunter_robot_demo.cpp

@@ -0,0 +1,99 @@
+/*
+ * hunter_robot_demo.cpp
+ *
+ * Created on: Jul 08, 2021 11:12
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#include <unistd.h>
+
+#include <memory>
+#include <iostream>
+
+#include "ugv_sdk/mobile_robot/hunter_robot.hpp"
+
+using namespace westonrobot;
+
+int main(int argc, char **argv) {
+  std::string protocol_version;
+  std::string device_name;
+
+  if (argc == 3) {
+    protocol_version = {argv[1]};
+    device_name = {argv[2]};
+    std::cout << "Use protocol " << protocol_version << " on interface "
+              << device_name << std::endl;
+  } else {
+    std::cout << "Usage: app_hunter_demo <protocol-version> <interface>"
+              << std::endl
+              << "Example 1: ./app_hunter_demo v1 can0" << std::endl;
+    return -1;
+  }
+
+  std::unique_ptr<HunterRobot> hunter;
+  if (protocol_version == "v1") {
+    hunter =
+        std::unique_ptr<HunterRobot>(new HunterRobot(ProtocolVersion::AGX_V1));
+  } else if (protocol_version == "v2") {
+    hunter =
+        std::unique_ptr<HunterRobot>(new HunterRobot(ProtocolVersion::AGX_V2));
+  } else {
+    std::cout << "Error: invalid protocol version string" << std::endl;
+    return -1;
+  }
+
+  if (hunter == nullptr)
+    std::cout << "Failed to create robot object" << std::endl;
+
+  hunter->Connect(device_name);
+
+  if (hunter->GetProtocolVersion() == ProtocolVersion::AGX_V2) {
+    hunter->EnableCommandedMode();
+    std::cout << "Protocol version 2" << std::endl;
+  } else {
+    std::cout << "Protocol version 1" << std::endl;
+  }
+
+  int count = 0;
+  while (true) {
+    // motion control
+    std::cout << "Motor: 1.0, 0" << std::endl;
+    hunter->SetMotionCommand(1.0, 0.0);
+
+    // get robot state
+    auto state = hunter->GetRobotState();
+    std::cout << "-------------------------------" << std::endl;
+    std::cout << "count: " << count << std::endl;
+    std::cout << "control mode: "
+              << static_cast<int>(state.system_state.control_mode)
+              << " , vehicle state: "
+              << static_cast<int>(state.system_state.vehicle_state)
+              << " , error code: " << std::hex << state.system_state.error_code
+              << ", battery voltage: " << state.system_state.battery_voltage
+              << std::endl;
+    std::cout << "velocity (linear, angular): "
+              << state.motion_state.linear_velocity << ", "
+              << state.motion_state.angular_velocity << std::endl;
+
+    auto actuator = hunter->GetActuatorState();
+    if (hunter->GetProtocolVersion() == ProtocolVersion::AGX_V1) {
+      for (int i = 0; i < 3; ++i) {
+        printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n",
+               actuator.actuator_state[i].motor_id,
+               actuator.actuator_state[i].current,
+               actuator.actuator_state[i].rpm,
+               actuator.actuator_state[i].driver_temp,
+               actuator.actuator_state[i].motor_temp);
+      }
+    } else {
+    }
+    std::cout << "-------------------------------" << std::endl;
+
+    usleep(20000);
+    ++count;
+  }
+
+  return 0;
+}

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 */

src/mobile_robot/hunter_robot.cpp

@@ -0,0 +1,50 @@
+/*
+ * hunter_robot.cpp
+ *
+ * Created on: Jul 14, 2021 23:30
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#include "ugv_sdk/mobile_robot/hunter_robot.hpp"
+#include "ugv_sdk/details/robot_base/hunter_base.hpp"
+
+namespace westonrobot {
+HunterRobot::HunterRobot(ProtocolVersion protocol) {
+  if (protocol == ProtocolVersion::AGX_V1) {
+    robot_ = new HunterBaseV1();
+  } else if (protocol == ProtocolVersion::AGX_V2) {
+    robot_ = new HunterBaseV2();
+  }
+}
+
+HunterRobot::~HunterRobot() {
+  if (robot_) delete robot_;
+}
+
+void HunterRobot::EnableCommandedMode() { robot_->EnableCommandedMode(); }
+
+void HunterRobot::Connect(std::string can_name) { robot_->Connect(can_name); }
+
+void HunterRobot::ResetRobotState() { robot_->ResetRobotState(); }
+
+ProtocolVersion HunterRobot::GetProtocolVersion() {
+  return robot_->GetProtocolVersion();
+}
+
+void HunterRobot::SetMotionCommand(double linear_vel, double angular_vel) {
+  auto hunter = dynamic_cast<HunterInterface*>(robot_);
+  hunter->SetMotionCommand(linear_vel, angular_vel);
+}
+
+HunterCoreState HunterRobot::GetRobotState() {
+  auto hunter = dynamic_cast<HunterInterface*>(robot_);
+  return hunter->GetRobotState();
+}
+
+HunterActuatorState HunterRobot::GetActuatorState() {
+  auto hunter = dynamic_cast<HunterInterface*>(robot_);
+  return hunter->GetActuatorState();
+}
+}  // namespace westonrobot