Merge pull request #1 from westonrobot/sample-v1.0.x

Updated for wrp_sdk v1.0.x

BUILD.md

@@ -0,0 +1,21 @@
+## Build the sample code
+
+### Install dependencies
+
+```
+$ sudo apt-get install -y software-properties-common
+$ sudo add-apt-repository ppa:lely/ppa && sudo apt-get update
+$ sudo apt-get install -y pkg-config liblely-coapp-dev liblely-co-tools
+```
+
+If you don't install the above dependencies (which are required by the peripheral drivers), only the robot base samples will be built.
+
+### Build the sample code 
+
+```
+$ mkdir build && cd build
+$ cmake ..
+$ make -j8
+```
+
+You will get executable "sample_xxx" inside the build/bin folder.

CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(wrp_sdk_sample)
+
+## Set compiler to use c++ 14 features
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## Add additional path for CMake to find wrp_sdk (IMPORTANT)
+list(APPEND CMAKE_PREFIX_PATH "/opt/weston_robot/lib/cmake")
+
+## Use GNUInstallDirs to install libraries into correct locations on all platforms.
+include(GNUInstallDirs)
+
+## Put all binary files into /bin and libraries into /lib
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR})
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR})
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_BINDIR})
+
+## Find wrp_sdk and its dependent package wrp_zbus
+find_package(wrp_sdk REQUIRED)
+
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(LelyIO QUIET IMPORTED_TARGET liblely-io2)
+pkg_check_modules(LelyCOAPP QUIET IMPORTED_TARGET liblely-coapp)
+
+set(BUILD_PERIPH_MODULE OFF)
+if(LelyIO_FOUND AND LelyCOAPP_FOUND)
+    message(STATUS "Lely-core libraries found, peripheral module will be included")
+    set(BUILD_PERIPH_MODULE ON)
+endif()
+
+## Add your target
+add_subdirectory(sample)

README.md

@@ -15,14 +15,22 @@ Please find the latest installation instructions from [this page](https://docs.w
 
 **Install the package**
 
+```
+$ sudo apt-get install -y software-properties-common
+$ sudo add-apt-repository ppa:lely/ppa && sudo apt-get update
+$ sudo apt-get install -y pkg-config liblely-coapp-dev liblely-co-tools
+```
+
 ```
 $ sudo apt-get install wrp_sdk
 ```
 
-**Note**: "apt-get" will install the latest version of wrp_sdk by default. Weston Robot releases stable versions of the SDK regularly. In case your robot firmware is not compatible with the latest SDK, you can install older versions of the SDK manually. For example,
+**Note**: "apt-get" will install the latest version of wrp_sdk by default. Weston Robot releases stable versions of the SDK regularly. In case your robot firmware is not compatible with the latest SDK, you can install older versions of the SDK manually. 
+
+For example, if your robot is only compatible with wrp_sdk up to v0.8.7, you can specify the version you want to install
 
 ```
-$ sudo apt-get remove wrp_zbus
+$ sudo apt-get remove wrp_zbus wrp_sdk
 $ sudo apt-get install wrp_zbus=0.6.1
 $ sudo apt-get install wrp_sdk=0.8.7
 ```
@@ -38,4 +46,4 @@ Please refer to the BUILD.md document in each "sample-*" branch for the building
 * **Q1:** What's the difference between [ugv_sdk](https://github.com/westonrobot/ugv_sdk) and wrp_sdk?
 
     **A1**: **ugv_sdk** provides a C++ interface to mobile robot bases manufactured by AgileX Robotics. It's a joint effort by Weston Robot and Agilex Robotics development teams. Robots compatible with ugv_sdk talk the AgileX Procotocol (V1 and V2) on the CAN bus. 
-    **wrp_sdk** provides a C++ interface to both robot bases and peripherals supported by Weston Robot. It allows users to use a single unified interface to control all different kinds of mobile robots (e.g. wheeled/tracked/legged robots), including but not limited to the ones from AgileX. It is also the foundation of the various robotics software toolboxes developed by Weston Robot (e.g. the Robot Maintenance Toolbox, the Assisted Driving Toolbox).
+    **wrp_sdk** provides a C++ interface to both robot bases and peripherals supported by Weston Robot. It allows users to use a single unified interface to control all different kinds of mobile robots (e.g. wheeled/tracked/legged robots), including but not limited to the ones from AgileX. It is also the foundation of the various robotics software toolboxes developed by Weston Robot (e.g. the Robot Maintenance Toolbox, the Assisted Driving Toolbox).

sample/CMakeLists.txt

@@ -0,0 +1,6 @@
+# sample
+add_subdirectory(mobile_base)
+
+if(BUILD_PERIPH_MODULE)
+add_subdirectory(peripheral)
+endif()

sample/mobile_base/CMakeLists.txt

@@ -0,0 +1,14 @@
+add_executable(sample_handshake_demo handshake_demo.cpp)
+target_link_libraries(sample_handshake_demo westonrobot::wrp_sdk)
+
+add_executable(sample_mobilebase_demo mobilebase_demo.cpp)
+target_link_libraries(sample_mobilebase_demo westonrobot::wrp_sdk)
+
+add_executable(sample_vbot_demo vbot_demo.cpp)
+target_link_libraries(sample_vbot_demo westonrobot::wrp_sdk)
+
+add_executable(sample_agilex_v2_robot_demo agilex_v2_robot_demo.cpp)
+target_link_libraries(sample_agilex_v2_robot_demo westonrobot::wrp_sdk)
+
+add_executable(sample_scout_v1_demo scout_v1_demo.cpp)
+target_link_libraries(sample_scout_v1_demo westonrobot::wrp_sdk)

sample/mobile_base/agilex_v2_robot_demo.cpp

@@ -0,0 +1,103 @@
+/* 
+ * agilex_v2_robot_demo.cpp
+ * 
+ * Created on: Jan 19, 2022 15:54
+ * Description: 
+ * 
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */ 
+
+#include <unistd.h>
+
+#include <iomanip>
+#include <sstream>
+#include <iostream>
+
+#include "wrp_sdk/mobile_base/agilex/agilex_base_v2_adapter.hpp"
+
+using namespace westonrobot;
+
+std::string ConvertToString(double value) {
+  std::stringstream stream;
+  stream << std::fixed << std::setprecision(2) << value;
+  return stream.str();
+}
+
+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: sample_agilex_v2_robot_demo <interface>" << std::endl
+              << "Example 1: ./sample_agilex_v2_robot_demo can0" << std::endl;
+    return -1;
+  }
+
+  std::unique_ptr<MobileRobotInterface> robot =
+      std::unique_ptr<MobileRobotInterface>(new AgilexBaseV2Adapter());
+  robot->Connect(device_name);
+
+  if (robot->RequestControl() != HandshakeReturnCode::kControlAcquired) {
+    std::cout << "Failed to gain control" << std::endl;
+    return -1;
+  }
+
+  uint8_t count = 0;
+  while (true) {
+    auto system_state = robot->GetSystemState();
+    auto motion_state = robot->GetMotionState();
+    auto odom = robot->GetOdometry();
+    auto battery = robot->GetBatteryState();
+
+    auto ultrasonic_data = robot->GetUltrasonicData();
+    auto tof_data = robot->GetTofData();
+
+    if (robot->SdkHasControlToken())
+      robot->SetMotionCommand({{1.0, 0, 0}, {0, 0, 0}});
+
+    // // set light command and query
+    // LightCommand light_command;
+    // light_command.id = 0;
+    // if (++count >= 100) {
+    //   MotionResetCommand motion_reset_command;
+    //   motion_reset_command.type = MotionResetCommandType::kOdometry;
+    //   robot->SetMotionResetCommand(motion_reset_command);
+    //   count = 0;
+    // } else if (++count >= 10) {
+    //   light_command.command.mode = LightMode::kEnquiry;
+    //   robot->SetLightCommand(light_command);
+    // } else if (++count >= 5) {
+    //   light_command.command.intensity = 20;
+    //   light_command.command.mode = LightMode::kCustom;
+    //   robot->SetLightCommand(light_command);
+    // }
+
+    // auto light_state = robot->GetLightState();
+    // std::cout << "Light State: " << (int)light_state.state.mode << " " <<
+    // light_state.state.intensity << std::endl;
+
+    std::cout << "Linear: " << ConvertToString(odom.linear.x) << ", ";
+    std::cout << "Angular: " << ConvertToString(odom.angular.z) << " ; ";
+
+    // std::cout << "\nUltrasonic:\n";
+    // for (int i = 0; i < 8; ++i)
+    //   std::cout << i << ":" << ultrasonic_data.data[i].range << std::endl;
+
+    std::cout << "\nSystemState:\n";
+    // std::cout << "Rc_connected: " << system_state.rc_connected << std::endl;
+    // std::cout << "Error_code: " << (int)system_state.error_code << std::endl;
+    // std::cout << "Operational_state: " << (int)system_state.operational_state
+    //           << std::endl;
+    // std::cout << "Control_state: " << (int)system_state.control_state
+    //           << std::endl;
+
+    // std::cout << "Desired_linear: " << motion_state.desired_linear.x << " "
+    //           << motion_state.desired_angular.z << " ; ";
+    std::cout << " Battery : " << battery.voltage << std::endl;
+
+    usleep(100000);
+  }
+  robot->RenounceControl();
+}

sample/mobile_base/handshake_demo.cpp

@@ -0,0 +1,87 @@
+/*
+ * handshake_demo.cpp
+ *
+ * Created on: Mar 10, 2021 10:35
+ * Description:
+ *
+ * Copyright (c) 2021 Weston Robot Pte. Ltd.
+ */
+
+#include <unistd.h>
+#include <iostream>
+
+#include "wrp_sdk/mobile_base/westonrobot/mobile_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: handshake_demo <interface>" << std::endl
+              << "Example 1: ./handshake_demo can0" << std::endl;
+    return -1;
+  }
+
+  MobileBase robot;
+  robot.Connect(device_name);
+
+  std::string line;
+  while (true) {
+    std::cout << "------------------------------------------------------"
+              << std::endl;
+    std::cout << "----Please input a char for the subsequent process----"
+              << std::endl;
+    std::cout << "r: "
+              << "request control\t"
+              << "d: "
+              << "renounce control\t" << std::endl;
+    std::cout << "s: "
+              << "speed control\t"
+              << "l: "
+              << "light control\t" << std::endl;
+    std::cin >> line;
+
+    if (line == "r") {
+      auto feedback = robot.RequestControl(500);
+      switch (feedback) {
+        case HandshakeReturnCode::kRobotBaseNotAlive:
+          std::cout << "RobotBaseNotAlive" << std::endl;
+          break;
+        case HandshakeReturnCode::kControlAcquired:
+          std::cout << "ControlAcquired" << std::endl;
+          break;
+        case HandshakeReturnCode::kControlRejected_RobotBaseFault:
+          std::cout << "ControlRejected_RobotBaseFault" << std::endl;
+          break;
+        case HandshakeReturnCode::kControlRejected_RcHaltTriggered:
+          std::cout << "ControlRejected_RcHaltTriggered" << std::endl;
+          break;
+        case HandshakeReturnCode::kControlRejected_RcControlActive:
+          std::cout << "ControlRejected_RcControlActive" << std::endl;
+          break;
+        case HandshakeReturnCode::kControlRejected_TokenTransferInterrupted:
+          std::cout << "ControlRejected_TokenTransferInterrupted" << std::endl;
+          break;
+        case HandshakeReturnCode::kControlRequestTimeout:
+          std::cout << "ControlRequestTimeout" << std::endl;
+          break;
+      }
+    } else if (line == "d") {
+      auto feedback = robot.RenounceControl(500);
+      switch (feedback) {
+        case HandshakeReturnCode::kControlHandedOver:
+          std::cout << "ControlHandedOver" << std::endl;
+          break;
+        case HandshakeReturnCode::kRenounceRequestTimeout:
+          std::cout << "RenounceRequestTimeout" << std::endl;
+          break;
+      }
+    }
+    usleep(100000);
+  }
+  return 0;
+}