added scout mini omni support, updated scout related demo

demo/scout_demo/CMakeLists.txt

@@ -1,2 +1,5 @@
 add_executable(demo_scout_robot scout_robot_demo.cpp)
 target_link_libraries(demo_scout_robot ugv_sdk)
+
+add_executable(demo_scout_mini_omni_robot scout_mini_omni_demo.cpp)
+target_link_libraries(demo_scout_mini_omni_robot ugv_sdk)

demo/scout_demo/scout_mini_omni_demo.cpp

@@ -0,0 +1,140 @@
+/*
+ * scout_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/scout_robot.hpp"
+#include "ugv_sdk/utilities/protocol_detector.hpp"
+
+using namespace westonrobot;
+
+int main(int argc, char **argv) {
+  std::string device_name;
+
+  if (argc == 2) {
+    device_name = {argv[1]};
+    std::cout << "Selected interface " << device_name << std::endl;
+  } else {
+    std::cout << "Usage: demo_scout_mini_omni_robot <interface>" << std::endl
+              << "Example 1: ./demo_scout_mini_omni_robot can0" << std::endl;
+    return -1;
+  }
+
+  std::unique_ptr<ScoutMiniOmniRobot> scout;
+
+  ProtocolDectctor detector;
+  detector.Connect("can0");
+  auto proto = detector.DetectProtocolVersion(5);
+  if (proto == ProtocolVersion::AGX_V1) {
+    std::cout << "Detected protocol: AGX_V1" << std::endl;
+    scout = std::unique_ptr<ScoutMiniOmniRobot>(
+        new ScoutMiniOmniRobot(ProtocolVersion::AGX_V1));
+
+  } else if (proto == ProtocolVersion::AGX_V2) {
+    std::cout << "Detected protocol: AGX_V2" << std::endl;
+    scout = std::unique_ptr<ScoutMiniOmniRobot>(
+        new ScoutMiniOmniRobot(ProtocolVersion::AGX_V2));
+  } else {
+    std::cout << "Detected protocol: UNKONWN" << std::endl;
+    return -1;
+  }
+
+  if (scout == nullptr)
+    std::cout << "Failed to create robot object" << std::endl;
+
+  scout->Connect(device_name);
+
+  if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V2) {
+    scout->EnableCommandedMode();
+  }
+
+  // light control
+  std::cout << "Light: const off" << std::endl;
+  scout->SetLightCommand(CONST_OFF, 0, CONST_OFF, 0);
+  sleep(3);
+  std::cout << "Light: const on" << std::endl;
+  scout->SetLightCommand(CONST_ON, 0, CONST_ON, 0);
+  sleep(3);
+  std::cout << "Light: breath" << std::endl;
+  scout->SetLightCommand(BREATH, 0, BREATH, 0);
+  sleep(3);
+  std::cout << "Light: custom 30-40" << std::endl;
+  scout->SetLightCommand(CUSTOM, 30, CUSTOM, 40);
+  sleep(3);
+  scout->SetLightCommand(CONST_OFF, 0, CONST_OFF, 0);
+
+  int count = 0;
+  while (true) {
+    // motion control
+    std::cout << "Setting motion command: 0.0, 0,0, 0.8" << std::endl;
+    scout->SetMotionCommand(0.0, 0.0, 0.8);
+
+    // get robot state
+    auto state = scout->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
+              << std::dec
+              << ", battery voltage: " << state.system_state.battery_voltage
+              << std::endl;
+    std::cout << "velocity (linear, angular, lateral): "
+              << state.motion_state.linear_velocity << ", "
+              << state.motion_state.angular_velocity << ", "
+              << state.motion_state.lateral_velocity << std::endl;
+    std::cout << "core state age (ms): "
+              << std::chrono::duration_cast<std::chrono::milliseconds>(
+                     AgxMsgRefClock::now() - state.time_stamp)
+                     .count()
+              << std::endl;
+
+    auto actuator = scout->GetActuatorState();
+    if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V1) {
+      for (int i = 0; i < 4; ++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);
+      }
+      std::cout << "actuator state age (ms): "
+                << std::chrono::duration_cast<std::chrono::milliseconds>(
+                       AgxMsgRefClock::now() - actuator.time_stamp)
+                       .count()
+                << std::endl;
+    } else {
+      for (int i = 0; i < 4; ++i) {
+        printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n",
+               actuator.actuator_hs_state[i].motor_id,
+               actuator.actuator_hs_state[i].current,
+               actuator.actuator_hs_state[i].rpm,
+               actuator.actuator_ls_state[i].driver_temp,
+               actuator.actuator_ls_state[i].motor_temp);
+      }
+      std::cout << "actuator state age (ms): "
+                << std::chrono::duration_cast<std::chrono::milliseconds>(
+                       AgxMsgRefClock::now() - actuator.time_stamp)
+                       .count()
+                << std::endl;
+    }
+    std::cout << "-------------------------------" << std::endl;
+
+    usleep(20000);
+    ++count;
+  }
+
+  return 0;
+}

demo/scout_demo/scout_robot_demo.cpp

@@ -19,17 +19,34 @@ using namespace westonrobot;
 
 int main(int argc, char **argv) {
   std::string device_name;
+  std::string robot_subtype;
 
   if (argc == 2) {
     device_name = {argv[1]};
-    std::cout << "Selected interface " << device_name << std::endl;
+    std::cout << "Selected interface " << device_name << ", robot type: scout"
+              << std::endl;
+  } else if (argc == 3) {
+    robot_subtype = {argv[1]};
+    device_name = {argv[2]};
+    std::cout << "Selected interface " << device_name
+              << ", robot type: " << robot_subtype << std::endl;
   } else {
-    std::cout << "Usage: app_scout_demo <protocol-version> <interface>"
+    std::cout << "Usage: demo_scout_robot [<robot-subtype>] <interface>"
               << std::endl
-              << "Example 1: ./app_scout_demo can0" << std::endl;
+              << "Example 1: ./demo_scout_robot can0" << std::endl
+              << "\t <robot-subtype>: mini" << std::endl;
     return -1;
   }
 
+  bool is_scout_mini = false;
+  if (robot_subtype == "mini") {
+    is_scout_mini = true;
+  } else if (!robot_subtype.empty() && robot_subtype != "scout") {
+    std::cout
+        << "Unkonwn robot subtype. Supported subtypes: \"scout\" or \"mini\""
+        << std::endl;
+  }
+
   std::unique_ptr<ScoutRobot> scout;
 
   ProtocolDectctor detector;
@@ -38,11 +55,11 @@ int main(int argc, char **argv) {
   if (proto == ProtocolVersion::AGX_V1) {
     std::cout << "Detected protocol: AGX_V1" << std::endl;
     scout = std::unique_ptr<ScoutRobot>(
-        new ScoutRobot(ProtocolVersion::AGX_V1, true));
+        new ScoutRobot(ProtocolVersion::AGX_V1, is_scout_mini));
   } else if (proto == ProtocolVersion::AGX_V2) {
     std::cout << "Detected protocol: AGX_V2" << std::endl;
     scout = std::unique_ptr<ScoutRobot>(
-        new ScoutRobot(ProtocolVersion::AGX_V2, true));
+        new ScoutRobot(ProtocolVersion::AGX_V2, is_scout_mini));
   } else {
     std::cout << "Detected protocol: UNKONWN" << std::endl;
     return -1;

include/ugv_sdk/details/interface/scout_interface.hpp

@@ -51,6 +51,13 @@ struct ScoutInterface {
   virtual ScoutCoreState GetRobotState() = 0;
   virtual ScoutActuatorState GetActuatorState() = 0;
 };
+
+struct ScoutOmniInterface {
+  virtual ~ScoutOmniInterface() = default;
+
+  virtual void SetMotionCommand(double linear_vel, double angular_vel,
+                                double lateral_velocity) = 0;
+};
 }  // namespace westonrobot
 
 #endif /* SCOUT_INTERFACE_HPP */

include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp

@@ -30,6 +30,8 @@ class ProtocolV1Parser : public ParserInterface<ProtocolVersion::AGX_V1> {
     if (msg->type == AgxMsgMotionCommandV1) {
       float linear = msg->body.v1_motion_command_msg.linear;
       float angular = msg->body.v1_motion_command_msg.angular;
+      float lateral = msg->body.v1_motion_command_msg.lateral;
+
       if (linear > RobotLimitsType::max_linear)
         linear = RobotLimitsType::max_linear;
       else if (linear < RobotLimitsType::min_linear)
@@ -38,11 +40,17 @@ class ProtocolV1Parser : public ParserInterface<ProtocolVersion::AGX_V1> {
         angular = RobotLimitsType::max_angular;
       else if (angular < RobotLimitsType::min_angular)
         angular = RobotLimitsType::min_angular;
+      if (lateral > RobotLimitsType::max_lateral)
+        lateral = RobotLimitsType::max_lateral;
+      else if (lateral < RobotLimitsType::min_lateral)
+        lateral = RobotLimitsType::min_lateral;
 
       msg_v1.body.v1_motion_command_msg.linear =
           linear / RobotLimitsType::max_linear * 100.0;
       msg_v1.body.v1_motion_command_msg.angular =
           angular / RobotLimitsType::max_angular * 100.0;
+      msg_v1.body.v1_motion_command_msg.lateral =
+          lateral / RobotLimitsType::max_lateral * 100.0;
     }
     return EncodeCanFrameV1(&msg_v1, tx_frame);
   }

include/ugv_sdk/details/protocol_v1/robot_limits.hpp

@@ -16,6 +16,9 @@ struct ScoutV2Limits {
   static constexpr double min_linear = -max_linear;
   static constexpr double max_angular = 0.5235;  // rad/s
   static constexpr double min_angular = -max_angular;
+
+  static constexpr double max_lateral = 0.0;
+  static constexpr double min_lateral = 0.0;
 };
 
 struct ScoutMiniLimits {
@@ -23,6 +26,9 @@ struct ScoutMiniLimits {
   static constexpr double min_linear = -max_linear;
   static constexpr double max_angular = 2.5235;  // rad/s
   static constexpr double min_angular = -max_angular;
+
+  static constexpr double max_lateral = 2.0;
+  static constexpr double min_lateral = -max_lateral;
 };
 
 struct BunkerLimits {
@@ -30,6 +36,9 @@ struct BunkerLimits {
   static constexpr double min_linear = -max_linear;
   static constexpr double max_angular = 2.5235;  // rad/s
   static constexpr double min_angular = -max_angular;
+
+  static constexpr double max_lateral = 0.0;
+  static constexpr double min_lateral = 0.0;
 };
 
 struct HunterV1Limits {
@@ -37,6 +46,9 @@ struct HunterV1Limits {
   static constexpr double min_linear = -max_linear;
   static constexpr double max_angular = 25.5;  // degree
   static constexpr double min_angular = -max_angular;
+
+  static constexpr double max_lateral = 0.0;
+  static constexpr double min_lateral = 0.0;
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -55,7 +55,7 @@ class AgilexBase : public RobotCommonInterface {
 
   // must be called at a frequency >= 50Hz
   void SendMotionCommand(double linear_vel, double angular_vel,
-                         double lateral_velocity, double steering_angle) {
+                         double lateral_vel, double steering_angle) {
     if (can_connected_) {
       // motion control message
       AgxMessage msg;
@@ -68,19 +68,18 @@ class AgilexBase : public RobotCommonInterface {
         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;
+        msg.body.v1_motion_command_msg.lateral = lateral_vel;
       } else if (parser_.GetParserProtocolVersion() ==
                  ProtocolVersion::AGX_V2) {
         msg.type = AgxMsgMotionCommand;
         msg.body.motion_command_msg.linear_velocity = linear_vel;
         msg.body.motion_command_msg.angular_velocity = angular_vel;
-        msg.body.motion_command_msg.lateral_velocity = lateral_velocity;
+        msg.body.motion_command_msg.lateral_velocity = lateral_vel;
         msg.body.motion_command_msg.steering_angle = steering_angle;
       }
 
-      //   std::cout << "sending motion cmd: " << linear_vel << "," <<
-      //   angular_vel
-      //             << std::endl;
+      std::cout << "sending motion cmd: " << linear_vel << "," << angular_vel
+                << "," << lateral_vel << std::endl;
 
       // send to can bus
       can_frame frame;

include/ugv_sdk/details/robot_base/scout_base.hpp

@@ -40,8 +40,9 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
                                               0.0);
   }
 
-  void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
-                       uint8_t r_value) override {
+  void SetLightCommand(LightMode f_mode, uint8_t f_value,
+                       LightMode r_mode = LightMode::CONST_ON,
+                       uint8_t r_value = 0) override {
     AgilexBase<ParserType>::SendLightCommand(f_mode, f_value, r_mode, r_value);
   }
 
@@ -71,6 +72,20 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
     return scout_actuator;
   }
 };
+
+template <typename ParserType>
+class ScoutMiniOmniBase : public ScoutBase<ParserType>,
+                          public ScoutOmniInterface {
+ public:
+  void SetMotionCommand(double linear_vel, double angular_vel,
+                        double lateral_velocity) override {
+    AgilexBase<ParserType>::SendMotionCommand(linear_vel, angular_vel,
+                                              lateral_velocity, 0.0);
+  }
+
+ private:
+  using ScoutBase<ParserType>::SetMotionCommand;
+};
 }  // namespace westonrobot
 
 #include "ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp"
@@ -79,9 +94,11 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
 namespace westonrobot {
 using ScoutBaseV1 = ScoutBase<ScoutProtocolV1Parser>;
 using ScoutMiniBaseV1 = ScoutBase<ScoutMiniProtocolV1Parser>;
+using ScoutMiniOmniBaseV1 = ScoutMiniOmniBase<ScoutMiniProtocolV1Parser>;
 
 using ScoutBaseV2 = ScoutBase<ProtocolV2Parser>;
 using ScoutMiniBaseV2 = ScoutBase<ProtocolV2Parser>;
+using ScoutMiniOmniBaseV2 = ScoutMiniOmniBase<ProtocolV2Parser>;
 }  // namespace westonrobot
 
 #endif /* SCOUT_BASE_HPP */

include/ugv_sdk/mobile_robot/scout_robot.hpp

@@ -20,7 +20,7 @@ class ScoutRobot : public RobotCommonInterface, public ScoutInterface {
  public:
   ScoutRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2,
              bool is_mini_model = false);
-  ~ScoutRobot();
+  virtual ~ScoutRobot();
 
   void Connect(std::string can_name) override;
   void Connect(std::string uart_name, uint32_t baudrate) override;
@@ -40,9 +40,24 @@ class ScoutRobot : public RobotCommonInterface, public ScoutInterface {
   ScoutCoreState GetRobotState() override;
   ScoutActuatorState GetActuatorState() override;
 
- private:
+ protected:
   RobotCommonInterface* robot_;
 };
+
+///////////////////////////////////////////////////////////////////////////
+
+class ScoutMiniOmniRobot : public ScoutRobot, public ScoutOmniInterface {
+ public:
+  ScoutMiniOmniRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2);
+  ~ScoutMiniOmniRobot();
+
+  void SetMotionCommand(double linear_vel, double angular_vel,
+                        double lateral_velocity) override;
+
+ private:
+  using ScoutRobot::SetMotionCommand;
+  //   void SetMotionCommand(double linear_vel, double angular_vel) = delete;
+};
 }  // namespace westonrobot
 
 #endif /* SCOUT_ROBOT_HPP */

src/mobile_robot/scout_robot.cpp

@@ -67,4 +67,24 @@ ScoutActuatorState ScoutRobot::GetActuatorState() {
   auto scout = dynamic_cast<ScoutInterface*>(robot_);
   return scout->GetActuatorState();
 }
+
+///////////////////////////////////////////////////////////////////////////
+
+ScoutMiniOmniRobot::ScoutMiniOmniRobot(ProtocolVersion protocol) {
+  if (protocol == ProtocolVersion::AGX_V1) {
+    robot_ = new ScoutMiniOmniBaseV1();
+  } else if (protocol == ProtocolVersion::AGX_V2) {
+    robot_ = new ScoutMiniOmniBaseV2();
+  }
+}
+
+ScoutMiniOmniRobot::~ScoutMiniOmniRobot() {
+  if (robot_) delete robot_;
+}
+
+void ScoutMiniOmniRobot::SetMotionCommand(double linear_vel, double angular_vel,
+                                          double lateral_velocity) {
+  auto scout = dynamic_cast<ScoutOmniInterface*>(robot_);
+  scout->SetMotionCommand(linear_vel, angular_vel, lateral_velocity);
+}
 }  // namespace westonrobot