saved work

CMakeLists.txt

@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 3.1.0)
 project(wrp_sdk)
 
-# # Find catkin
+# Find catkin
-# find_package(catkin REQUIRED)
+find_package(catkin REQUIRED)
 
 # generate symbols for IDE indexer
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
@@ -14,12 +14,12 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
 ## Put all binary files into /bin and libraries into /lib
-set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
+# set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
-set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
+# set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
-set(ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
+# set(ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
 
 ## Chosse build type
-set(CMAKE_BUILD_TYPE Release)
+# set(CMAKE_BUILD_TYPE Release)
 # set(CMAKE_BUILD_TYPE Debug)
 
 set(default_build_type "Release")
@@ -33,7 +33,7 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
 endif()
 
 ## Optionally built modules: ON/OFF
-set(BUILD_TESTS ON)
+set(BUILD_TESTS OFF)
 set(BUILD_MONITOR ON)
 
 # Disable monitor if ncurses library is not found
@@ -46,7 +46,20 @@ if(BUILD_MONITOR AND NOT CURSES_FOUND)
 endif()
 
 # Add libraries
-set(WRP_SDK_SRC
+# set(WRP_SDK_SRC
+#     src/async_serial.cpp
+#     src/async_can.cpp
+#     src/asyncio_utils.cpp
+#     # agilex mobile platforms
+#     src/platforms/mobile_base.cpp
+#     src/platforms/hunter_base.cpp
+#     src/platforms/hunter_can_parser.c
+#     src/platforms/scout_base.cpp
+#     src/platforms/scout_can_parser.c
+#     src/platforms/scout_uart_parser.c
+# )
+# add_library(${PROJECT_NAME} STATIC ${WRP_SDK_SRC})
+add_library(${PROJECT_NAME} 
     src/async_serial.cpp
     src/async_can.cpp
     src/asyncio_utils.cpp
@@ -58,65 +71,36 @@ set(WRP_SDK_SRC
     src/platforms/scout_can_parser.c
     src/platforms/scout_uart_parser.c
 )
-add_library(wrpsdk STATIC ${WRP_SDK_SRC})
+target_compile_definitions(${PROJECT_NAME} PUBLIC "-DASIO_ENABLE_OLD_SERVICES -DASIO_HAS_POSIX_STREAM_DESCRIPTOR")
-target_compile_definitions(wrpsdk PUBLIC "-DASIO_ENABLE_OLD_SERVICES -DASIO_HAS_POSIX_STREAM_DESCRIPTOR")
+target_link_libraries(${PROJECT_NAME} pthread)
-target_link_libraries(wrpsdk pthread)
+target_include_directories(${PROJECT_NAME} PUBLIC
-target_include_directories(wrpsdk PUBLIC
 	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 	$<INSTALL_INTERFACE:include>
 	PRIVATE src)
 
-# install(TARGETS asyncio
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
-
-# install(DIRECTORY include asio
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
-
 # Add executables
 if(BUILD_TESTS)
-	add_subdirectory(tests)
+    add_subdirectory(tests)
+    add_subdirectory(unit_tests)
 endif()
 
-
+## Export catkin targets
-if(BUILD_TESTS)
+catkin_package(
-add_subdirectory(unit_tests)
+ INCLUDE_DIRS include
-endif()
+ LIBRARIES ${PROJECT_NAME}
+#  CATKIN_DEPENDS hunter_msgs roscpp 
+#  DEPENDS system_lib
+)
 
 ## Use GNUInstallDirs to install libraries into correct
 # locations on all platforms.
 include(GNUInstallDirs)
 
-# ## Export catkin targets
-# set(EXPORTED_LIBS 
-#     asyncio
-#     stopwatch
-#     scout_protocol
-#     scoutbase
-#     hunter_protocol
-#     hunterbase
-# )
-# set(INCLUDE_DIRS 
-#     src/common/async_io/asio
-#     src/common/async_io/include
-#     src/common/utilities/stopwatch/include
-#     src/scout_sdk/scout_protocol/include
-#     src/scout_sdk/scout_base/include
-#     src/hunter_sdk/hunter_protocol/include
-#     src/hunter_sdk/hunter_base/include
-# )
-# catkin_package(
-#     LIBRARIES ${EXPORTED_LIBS}
-#     INCLUDE_DIRS ${INCLUDE_DIRS}
-#     # DEPENDS rt pthread
-# )
-
 ## Add catkin install targets
-# install(TARGETS ${EXPORTED_LIBS}
+install(TARGETS ${PROJECT_NAME}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
 
 # install(DIRECTORY include/${PROJECT_NAME}/
 #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})

include/wrp_sdk/platforms/common/mobile_base.hpp

@@ -17,7 +17,6 @@
 #include <cstdint>
 #include <thread>
 #include <mutex>
-#include <functional>
 
 #include "wrp_sdk/asyncio/async_can.hpp"
 #include "wrp_sdk/asyncio/async_serial.hpp"
@@ -64,10 +63,11 @@ class MobileBase {
   void StartCmdThread();
   void ControlLoop(int32_t period_ms);
 
+  // functions that must/may be implemented by child classes
   virtual void SendRobotCmd() = 0;
-  virtual void ParseCANFrame(can_frame *rx_frame) = 0;
+  virtual void ParseCANFrame(can_frame *rx_frame){};
   virtual void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
-                               size_t bytes_received) = 0;
+                               size_t bytes_received){};
 };
 }  // namespace westonrobot
 

include/wrp_sdk/platforms/hunter/hunter_base.hpp

@@ -14,84 +14,50 @@
 #include <cstdint>
 #include <thread>
 #include <mutex>
-#include <functional>
 
-#include "wrp_sdk/asyncio/async_can.hpp"
+#include "wrp_sdk/platforms/common/mobile_base.hpp"
-#include "wrp_sdk/asyncio/async_serial.hpp"
 
 #include "wrp_sdk/platforms/hunter/hunter_protocol.h"
 #include "wrp_sdk/platforms/hunter/hunter_can_parser.h"
-
 #include "wrp_sdk/platforms/hunter/hunter_types.hpp"
 
 namespace westonrobot {
-class HunterBase {
+class HunterBase : public MobileBase {
  public:
-  HunterBase() = default;
+  HunterBase() : MobileBase(){};
-  ~HunterBase();
+  ~HunterBase() = default;
 
-  // do not allow copy
+  // get robot state
-  HunterBase(const HunterBase &hunter) = delete;
+  HunterState GetHunterState();
-  HunterBase &operator=(const HunterBase &hunter) = delete;
-
- public:
-  // connect to roboot from CAN
-  void Connect(std::string dev_name);
-
-  // disconnect from roboot, only valid for serial port
-  void Disconnect();
-
-  // cmd thread runs at 100Hz (10ms) by default
-  void SetCmdThreadPeriodMs(int32_t period_ms) {
-    cmd_thread_period_ms_ = period_ms;
-  };
 
   // motion control
   void SetMotionCommand(double linear_vel, double steering_angle,
                         HunterMotionCmd::FaultClearFlag fault_clr_flag =
                             HunterMotionCmd::FaultClearFlag::NO_FAULT);
 
-  // get robot state
-  HunterState GetHunterState();
-
  private:
-  // hardware communication interface
-  std::shared_ptr<ASyncCAN> can_if_;
-  std::shared_ptr<ASyncSerial> serial_if_;
-
-  // CAN priority higher than serial if both connected
-  bool can_connected_ = false;
-  bool serial_connected_ = false;
-
   // serial port related variables
   uint8_t tx_cmd_len_;
   uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN];
 
   // cmd/status update related variables
-  std::thread cmd_thread_;
   std::mutex hunter_state_mutex_;
   std::mutex motion_cmd_mutex_;
 
   HunterState hunter_state_;
   HunterMotionCmd current_motion_cmd_;
 
-  int32_t cmd_thread_period_ms_ = 10;
-  bool cmd_thread_started_ = false;
-
   // internal functions
   void ConfigureCANBus(const std::string &can_if_name = "can1");
   void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0",
                        int32_t baud_rate = 115200);
 
-  void StartCmdThread();
+  void SendRobotCmd() override;
-  void ControlLoop(int32_t period_ms);
+  void ParseCANFrame(can_frame *rx_frame) override;
+  void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
+                       size_t bytes_received) override{};
 
   void SendMotionCmd(uint8_t count);
-
-  void ParseCANFrame(can_frame *rx_frame);
-  void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
-                       size_t bytes_received);
-
   void NewStatusMsgReceivedCallback(const HunterMessage &msg);
 
  public:

src/platforms/hunter_base.cpp

@@ -13,60 +13,8 @@
 #include "stopwatch.h"
 
 namespace westonrobot {
-HunterBase::~HunterBase() {
-  if (serial_connected_) serial_if_->close();
 
-  if (cmd_thread_.joinable()) cmd_thread_.join();
+void HunterBase::SendRobotCmd() {}
-}
-
-void HunterBase::Connect(std::string dev_name) {
-  //   if (baud_rate == 0) {
-  ConfigureCANBus(dev_name);
-  //   } else {
-  //     ConfigureSerial(dev_name, baud_rate);
-
-  //     if (!serial_connected_)
-  //       std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
-  //   }
-}
-
-void HunterBase::Disconnect() {
-  if (serial_connected_) {
-    if (serial_if_->is_open()) serial_if_->close();
-  }
-}
-
-void HunterBase::ConfigureCANBus(const std::string &can_if_name) {
-  can_if_ = std::make_shared<ASyncCAN>(can_if_name);
-
-  can_if_->set_receive_callback(
-      std::bind(&HunterBase::ParseCANFrame, this, std::placeholders::_1));
-
-  can_connected_ = true;
-}
-
-void HunterBase::ConfigureSerial(const std::string uart_name,
-                                 int32_t baud_rate) {
-  serial_if_ = std::make_shared<ASyncSerial>(uart_name, baud_rate);
-  serial_if_->open();
-
-  if (serial_if_->is_open()) serial_connected_ = true;
-
-  serial_if_->set_receive_callback(
-      std::bind(&HunterBase::ParseUARTBuffer, this, std::placeholders::_1,
-                std::placeholders::_2, std::placeholders::_3));
-}
-
-void HunterBase::StartCmdThread() {
-  current_motion_cmd_.linear_velocity = 0;
-  current_motion_cmd_.angular_velocity = 0;
-  current_motion_cmd_.fault_clear_flag =
-      HunterMotionCmd::FaultClearFlag::NO_FAULT;
-
-  cmd_thread_ = std::thread(
-      std::bind(&HunterBase::ControlLoop, this, cmd_thread_period_ms_));
-  cmd_thread_started_ = true;
-}
 
 void HunterBase::SendMotionCmd(uint8_t count) {
   // motion control message
@@ -110,22 +58,6 @@ void HunterBase::SendMotionCmd(uint8_t count) {
   }
 }
 
-void HunterBase::ControlLoop(int32_t period_ms) {
-  StopWatch ctrl_sw;
-  uint8_t cmd_count = 0;
-  uint8_t light_cmd_count = 0;
-  while (true) {
-    ctrl_sw.tic();
-
-    // motion control message
-    SendMotionCmd(cmd_count++);
-
-    ctrl_sw.sleep_until_ms(period_ms);
-    // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() <<
-    // std::endl;
-  }
-}
-
 HunterState HunterBase::GetHunterState() {
   std::lock_guard<std::mutex> guard(hunter_state_mutex_);
   return hunter_state_;
@@ -170,20 +102,6 @@ void HunterBase::ParseCANFrame(can_frame *rx_frame) {
   NewStatusMsgReceivedCallback(status_msg);
 }
 
-void HunterBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
-                                 size_t bytes_received) {
-  // std::cout << "bytes received from serial: " << bytes_received << std::endl;
-  // serial_parser_.PrintStatistics();
-  // serial_parser_.ParseBuffer(buf, bytes_received);
-
-  // TODO
-  //   HunterMessage status_msg;
-  //   for (int i = 0; i < bytes_received; ++i) {
-  //     if (DecodeHunterMsgFromUART(buf[i], &status_msg))
-  //       NewStatusMsgReceivedCallback(status_msg);
-  //   }
-}
-
 void HunterBase::NewStatusMsgReceivedCallback(const HunterMessage &msg) {
   // std::cout << "new status msg received" << std::endl;
   std::lock_guard<std::mutex> guard(hunter_state_mutex_);

src/platforms/scout_base.cpp

@@ -335,4 +335,4 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, ScoutState &sta
     }
     }
 }
-} // namespace wescore
+} // namespace westonrobot