add ranger vehicle

CMakeLists.txt

@@ -89,6 +89,7 @@ add_library(${PROJECT_NAME}
     # src/hunter_base.cpp
     src/tracer_base.cpp
     # src/bunker_base.cpp
+    src/ranger_base.cpp
 )
 if(BUILD_WITHOUT_ROS)
     target_link_libraries(${PROJECT_NAME} westonrobot::async_port Threads::Threads)

apps/CMakeLists.txt

@@ -14,6 +14,9 @@
 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)
+
 # if(BUILD_MONITOR)
 #     add_subdirectory(scout_monitor)
 # endif()

apps/ranger_demo/ranger_demo.cpp

@@ -0,0 +1,57 @@
+/**
+ * @Kit       : Qt-Creator: Desktop
+ * @Author    : Wang Zhe
+ * @Date      : 2021-04-19  19:41:52
+ * @FileName  : ranger_demo.cpp
+ * @Mail      : wangzheqie@qq.com
+ * Copyright  : AgileX Robotics
+ **/
+
+#include "ugv_sdk/ranger_base.hpp"
+
+using namespace westonrobot;
+
+int main(int argc, char *argv[]) {
+  std::string device_name;
+
+  if (argc == 2) {
+    device_name = {argv[1]};
+    std::cout << "Specified CAN: " << device_name << std::endl;
+  } else {
+    std::cout << "Usage: app_ranger_demo <interface>" << std::endl
+              << "Example 1: ./app_ranger_demo can0" << std::endl;
+    return -1;
+  }
+
+  RangerBase ranger;
+  ranger.Connect(device_name);
+
+  ranger.EnableCommandedMode();
+
+  int count = 0;
+  while (true) {
+    if (count < 100) {
+      std::cout << "Motor: 0.2, 0" << std::endl;
+      ranger.SetMotionCommand(0.2, 0.0);
+    }
+
+    auto state = ranger.GetRangerState();
+    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)
+              << std::endl;
+    std::cout << "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/ranger_base.hpp

@@ -0,0 +1,82 @@
+/**
+ * @Kit       : Qt-Creator: Desktop
+ * @Author    : Wang Zhe
+ * @Date      : 2021-04-19  19:12:52
+ * @FileName  : ranger_base.hpp
+ * @Mail      : wangzheqie@qq.com
+ * Copyright  : AgileX Robotics
+ **/
+
+#ifndef RANGER_BASE_HPP
+#define RANGER_BASE_HPP
+
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#include "ugv_sdk/agilex_base.hpp"
+
+namespace westonrobot {
+struct RangerState {
+  // system state
+  SystemStateMessage system_state;
+  MotionStateMessage motion_state;
+  LightStateMessage light_state;
+
+  RcStateMessage rc_state;
+
+  ActuatorHSStateMessage actuator_hs_state[4];
+  ActuatorLSStateMessage actuator_ls_state[4];
+
+  // sensor data
+  OdometryMessage odometry;
+};
+
+struct RangerMotionCmd {
+  double linear_velocity;
+  double angular_velocity;
+};
+
+struct RangerLightCmd {
+  RangerLightCmd() = default;
+  RangerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
+                 uint8_t r_value)
+      : cmd_ctrl_allowed(true),
+        front_mode(f_mode),
+        front_custom_value(f_value),
+        rear_mode(r_mode),
+        rear_custom_value(r_value) {}
+
+  bool cmd_ctrl_allowed = false;
+  LightMode front_mode;
+  uint8_t front_custom_value;
+  LightMode rear_mode;
+  uint8_t rear_custom_value;
+};
+
+/////////////////////////////////////////////////////////////////////////
+
+class RangerBase : public AgilexBase {
+ public:
+  RangerBase() : AgilexBase(){};
+  ~RangerBase() = default;
+
+  // set up connection
+  void Connect(std::string dev_name) override;
+
+  // robot control
+  void SetMotionCommand(double linear_vel, double angular_vel);
+  void SetLightCommand(const RangerLightCmd &cmd);
+
+  // get robot state
+  RangerState GetRangerState();
+
+ private:
+  RangerState ranger_state_;
+
+  void ParseCANFrame(can_frame *rx_frame) override;
+  void UpdateRangerState(const AgxMessage &status_msg, RangerState &state);
+};
+}  // namespace westonrobot
+#endif  // RANGER_BASE_HPP

src/ranger_base.cpp

@@ -0,0 +1,98 @@
+/**
+ * @Kit       : Qt-Creator: Desktop
+ * @Author    : Wang Zhe
+ * @Date      : 2021-04-19  19:36:24
+ * @FileName  : ranger_base.cpp
+ * @Mail      : wangzheqie@qq.com
+ * Copyright  : AgileX Robotics
+ **/
+
+#include "ugv_sdk/ranger_base.hpp"
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <ratio>
+#include <string>
+#include <thread>
+
+#include "stopwatch.hpp"
+
+#include "ugv_sdk/details/agilex_msg_parser.h"
+
+namespace westonrobot {
+void RangerBase::Connect(std::string dev_name) {
+  AgilexBase::Connect(dev_name, std::bind(&RangerBase::ParseCANFrame, this,
+                                          std::placeholders::_1));
+}
+
+void RangerBase::SetMotionCommand(double linear_vel, double angular_vel) {
+  AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
+}
+
+void RangerBase::SetLightCommand(const RangerLightCmd &cmd) {
+  if (cmd.cmd_ctrl_allowed) {
+    AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
+                                 LightMode::CONST_OFF, 0);
+  }
+}
+
+RangerState RangerBase::GetRangerState() {
+  std::lock_guard<std::mutex> guard(state_mutex_);
+  return ranger_state_;
+}
+
+void RangerBase::ParseCANFrame(can_frame *rx_frame) {
+  AgxMessage status_msg;
+  DecodeCanFrame(rx_frame, &status_msg);
+  std::lock_guard<std::mutex> guard(state_mutex_);
+  UpdateRangerState(status_msg, ranger_state_);
+}
+
+void RangerBase::UpdateRangerState(const AgxMessage &status_msg,
+                                 RangerState &state) {
+  switch (status_msg.type) {
+    case AgxMsgSystemState: {
+      //   std::cout << "system status feedback received" << std::endl;
+      state.system_state = status_msg.body.system_state_msg;
+      break;
+    }
+    case AgxMsgMotionState: {
+      // std::cout << "motion control feedback received" << std::endl;
+      state.motion_state = status_msg.body.motion_state_msg;
+      break;
+    }
+    case AgxMsgLightState: {
+      // std::cout << "light control feedback received" << std::endl;
+      state.light_state = status_msg.body.light_state_msg;
+      break;
+    }
+    case AgxMsgRcState: {
+      state.rc_state = status_msg.body.rc_state_msg;
+      break;
+    }
+    case AgxMsgActuatorHSState: {
+      // std::cout << "actuator hs feedback received" << std::endl;
+      state.actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] =
+          status_msg.body.actuator_hs_state_msg;
+      break;
+    }
+    case AgxMsgActuatorLSState: {
+      // std::cout << "actuator ls feedback received" << std::endl;
+      state.actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
+          status_msg.body.actuator_ls_state_msg;
+      break;
+    }
+    /* sensor feedback */
+    case AgxMsgOdometry: {
+      // std::cout << "Odometer msg feedback received" << std::endl;
+      state.odometry = status_msg.body.odometry_msg;
+    }
+    default:
+      break;
+  }
+}
+}  // namespace westonrobot