added bunker robot support

CMakeLists.txt

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

demo/CMakeLists.txt

@@ -2,6 +2,7 @@
 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)

demo/bunker_demo/CMakeLists.txt

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

demo/bunker_demo/bunker_robot_demo.cpp

@@ -0,0 +1,86 @@
+/*
+ * bunker_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/bunker_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_bunker_demo <protocol-version> <interface>"
+              << std::endl
+              << "Example 1: ./app_bunker_demo v1 can0" << std::endl;
+    return -1;
+  }
+
+  std::unique_ptr<BunkerRobot> bunker;
+  if (protocol_version == "v1") {
+    bunker =
+        std::unique_ptr<BunkerRobot>(new BunkerRobot(ProtocolVersion::AGX_V1));
+  } else if (protocol_version == "v2") {
+    bunker =
+        std::unique_ptr<BunkerRobot>(new BunkerRobot(ProtocolVersion::AGX_V2));
+  } else {
+    std::cout << "Error: invalid protocol version string" << std::endl;
+    return -1;
+  }
+
+  if (bunker == nullptr)
+    std::cout << "Failed to create robot object" << std::endl;
+
+  bunker->Connect(device_name);
+
+  if (bunker->GetProtocolVersion() == ProtocolVersion::AGX_V2) {
+    bunker->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;
+    bunker->SetMotionCommand(1.0, 0.0);
+
+    // get robot state
+    auto state = bunker->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;
+    std::cout << "-------------------------------" << std::endl;
+
+    usleep(20000);
+    ++count;
+  }
+
+  return 0;
+}

include/ugv_sdk/details/interface/bunker_interface.hpp

@@ -0,0 +1,34 @@
+/*
+ * bunker_interface.hpp
+ *
+ * Created on: Jul 14, 2021 23:04
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef BUNKER_INTERFACE_HPP
+#define BUNKER_INTERFACE_HPP
+
+#include <string>
+
+#include "ugv_sdk/details/interface/agilex_message.h"
+
+namespace westonrobot {
+struct BunkerCoreState {
+  SystemStateMessage system_state;
+  MotionStateMessage motion_state;
+  RcStateMessage rc_state;
+};
+
+struct BunkerInterface {
+  virtual ~BunkerInterface() = default;
+
+  virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
+
+  // get robot state
+  virtual BunkerCoreState GetRobotState() = 0;
+};
+}  // namespace westonrobot
+
+#endif /* BUNKER_INTERFACE_HPP */

include/ugv_sdk/details/interface/robot_interface.hpp

@@ -40,7 +40,6 @@ class RobotInterface {
 
   // functions to be implemented by class AgilexBase
   virtual void EnableCommandedMode() = 0;
-  virtual void DisableLightControl() = 0;
 
   // functions to be implemented by each robot class
   virtual void Connect(std::string can_name) = 0;
@@ -50,6 +49,10 @@ class RobotInterface {
   };
   virtual void ResetRobotState() = 0;
 
+  virtual void DisableLightControl() {
+    // use derived version
+  }
+
   virtual ProtocolVersion GetProtocolVersion() = 0;
 
  protected:
@@ -72,9 +75,7 @@ class RobotInterface {
 
   virtual void SendLightCommand(LightMode front_mode,
                                 uint8_t front_custom_value, LightMode rear_mode,
-                                uint8_t rear_custom_value){
+                                uint8_t rear_custom_value){};
-      // use derived version
-  };
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp

@@ -65,6 +65,8 @@ class ProtocolV1Parser : public ParserInterface<ProtocolVersion::AGX_V1> {
 using ScoutProtocolV1Parser = ProtocolV1Parser<ScoutV2Limits>;
 using ScoutMiniProtocolV1Parser = ProtocolV1Parser<ScoutMiniLimits>;
 
+using BunkerProtocolV1Parser = ProtocolV1Parser<BunkerLimits>;
+
 using HunterProtocolV1Parser = ProtocolV1Parser<HunterV1Limits>;
 
 }  // namespace westonrobot

include/ugv_sdk/details/protocol_v1/robot_limits.hpp

@@ -25,6 +25,13 @@ struct ScoutMiniLimits {
   static constexpr double min_angular = -max_angular;
 };
 
+struct BunkerLimits {
+  static constexpr double max_linear = 3.0;  // m/s
+  static constexpr double min_linear = -max_linear;
+  static constexpr double max_angular = 2.5235;  // rad/s
+  static constexpr double min_angular = -max_angular;
+};
+
 struct HunterV1Limits {
   static constexpr double max_linear = 1.5;  // m/s
   static constexpr double min_linear = -max_linear;

include/ugv_sdk/details/robot_base/bunker_base.hpp

@@ -0,0 +1,60 @@
+/*
+ * bunker_base.hpp
+ *
+ * Created on: Jul 14, 2021 23:05
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef BUNKER_BASE_HPP
+#define BUNKER_BASE_HPP
+
+#include <string>
+#include <cstdint>
+#include <thread>
+#include <mutex>
+
+#include "ugv_sdk/details/interface/bunker_interface.hpp"
+#include "ugv_sdk/details/robot_base/agilex_base.hpp"
+
+namespace westonrobot {
+template <typename ParserType>
+class BunkerBase : public AgilexBase<ParserType>, public BunkerInterface {
+ public:
+  BunkerBase() : AgilexBase<ParserType>(){};
+  ~BunkerBase() = 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, angular_vel, 0.0,
+                                              0.0);
+  }
+
+  // get robot state
+  BunkerCoreState GetRobotState() override {
+    auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
+
+    BunkerCoreState bunker_state;
+    bunker_state.system_state = state.system_state;
+    bunker_state.motion_state = state.motion_state;
+    bunker_state.rc_state = state.rc_state;
+    return bunker_state;
+  }
+};
+}  // 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 BunkerBaseV1 = BunkerBase<BunkerProtocolV1Parser>;
+using BunkerBaseV2 = BunkerBase<ProtocolV2Parser>;
+}  // namespace westonrobot
+
+#endif /* BUNKER_BASE_HPP */

include/ugv_sdk/mobile_robot/bunker_robot.hpp

@@ -0,0 +1,42 @@
+/*
+ * bunker_robot.hpp
+ *
+ * Created on: Jul 14, 2021 23:08
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef BUNKER_ROBOT_HPP
+#define BUNKER_ROBOT_HPP
+
+#include <memory>
+
+#include "ugv_sdk/details/interface/robot_interface.hpp"
+#include "ugv_sdk/details/interface/bunker_interface.hpp"
+
+namespace westonrobot {
+class BunkerRobot : public RobotInterface, public BunkerInterface {
+ public:
+  BunkerRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2);
+  ~BunkerRobot();
+
+  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
+  BunkerCoreState GetRobotState() override;
+
+ private:
+  RobotInterface* robot_;
+};
+}  // namespace westonrobot
+
+#endif /* BUNKER_ROBOT_HPP */

src/mobile_robot/bunker_robot.cpp

@@ -0,0 +1,45 @@
+/*
+ * bunker_robot.cpp
+ *
+ * Created on: Jul 14, 2021 23:14
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#include "ugv_sdk/mobile_robot/bunker_robot.hpp"
+#include "ugv_sdk/details/robot_base/bunker_base.hpp"
+
+namespace westonrobot {
+BunkerRobot::BunkerRobot(ProtocolVersion protocol) {
+  if (protocol == ProtocolVersion::AGX_V1) {
+    robot_ = new BunkerBaseV1();
+  } else if (protocol == ProtocolVersion::AGX_V2) {
+    robot_ = new BunkerBaseV2();
+  }
+}
+
+BunkerRobot::~BunkerRobot() {
+  if (robot_) delete robot_;
+}
+
+void BunkerRobot::EnableCommandedMode() { robot_->EnableCommandedMode(); }
+
+void BunkerRobot::Connect(std::string can_name) { robot_->Connect(can_name); }
+
+void BunkerRobot::ResetRobotState() { robot_->ResetRobotState(); }
+
+ProtocolVersion BunkerRobot::GetProtocolVersion() {
+  return robot_->GetProtocolVersion();
+}
+
+void BunkerRobot::SetMotionCommand(double linear_vel, double angular_vel) {
+  auto bunker = dynamic_cast<BunkerInterface*>(robot_);
+  bunker->SetMotionCommand(linear_vel, angular_vel);
+}
+
+BunkerCoreState BunkerRobot::GetRobotState() {
+  auto bunker = dynamic_cast<BunkerInterface*>(robot_);
+  return bunker->GetRobotState();
+}
+}  // namespace westonrobot