major revision

NOTE.md

@@ -0,0 +1,19 @@
+```
+$ sudo apt-get install ros-kinetic-joint-state-controller
+$ sudo apt-get install ros-kinetic-effort-controllers
+$ sudo apt-get install ros-kinetic-position-controllers
+```
+
+```
+$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=scout_robot/cmd_vel
+```
+
+xacro to URDF
+```
+$ rosrun xacro xacro scout.urdf.xacro > agilex_scout.urdf
+```
+
+URDF to PROTO
+```
+$ python urdf2webots.py --input=someRobot.urdf [--output=outputFile] [--box-collision] [--normal]
+```

README.md

@@ -2,4 +2,6 @@
 
 ## Packages
 
-* scout_base: a ROS wrapper around Scout SDK to monitor and command the robot
+* scout_bringup: launch and configuration files to start ROS nodes 
+* scout_base: a ROS wrapper around Scout SDK to monitor and control the robot
+* scout_msgs: scout related message definitions

scout_base/.gitignore

@@ -1,101 +0,0 @@
-# build
-*/Debug
-build
-*/build
-cmake-build-debug
-cmake-build-release/
-
-# Temp files
-*~
-*/~
-*/*.*~
-*.m~
-*.DS_Store
-
-# log
-*.log
-#*.data
-
-# Doxygen
-docs/doxygen/html/
-docs/doxygen/latex
-
-# Qt Creator
-pc/visualization/CMakeLists.txt.user
-
-# VS Code
-.vscode
-
-# CLion
-.idea/
-
-# LCM
-src/lcmtypes/c
-src/lcmtypes/cpp
-src/lcmtypes/java
-src/lcmtypes/python
-
-# Compiled Object files
-*.slo
-*.lo
-*.o
-*.obj
-
-# Precompiled Headers
-*.gch
-*.pch
-
-# Compiled Dynamic libraries
-#*.so
-#*.dylib
-#*.dll
-
-# Fortran module files
-*.mod
-
-# Compiled Static libraries
-#*.lai
-#*.la
-#*.a
-#*.lib
-
-# Executables
-*.exe
-*.out
-*.app
-
-# Vim
-tags
-*/tags
-*.swp
-*/*.swp
-
-# vscode
-.vscode
-
-# Python
-*.pyc
-__pycache__
-
-##---------------------------------------------------
-## Remove autosaves generated by the Matlab editor
-## We have git for backups!
-##---------------------------------------------------
-
-# Windows default autosave extension
-*.asv
-
-# OSX / *nix default autosave extension
-*.m~
-
-# Compiled MEX binaries (all platforms)
-*.mex*
-
-# Simulink Code Generation
-slprj/
-
-# Session info
-octave-workspace
-
-# Simulink autosave extension
-.autosave

scout_base/CHANGELOG.md

@@ -0,0 +1,12 @@
+# Changelog for package scout_base
+
+## 0.0.2 (2019-06-14)
+------------------
+* Deprecated initial serial interface support (new one under development)
+* Based on Scout SDK v0.2 with CAN interface 
+* Contributors: Ruixiang Du
+  
+## 0.0.1 (2019-05-08)
+------------------
+* Initial development of scout_base for Scout
+* Contributors: Ruixiang Du

scout_base/CMakeLists.txt

@@ -7,16 +7,16 @@ add_compile_options(-std=c++11)
 find_package(catkin REQUIRED COMPONENTS 
     controller_manager 
     hardware_interface 
-    husky_msgs
-    diagnostic_updater 
     roslaunch 
     roslint 
     roscpp 
     sensor_msgs
     std_msgs
     geometry_msgs
+    scout_msgs
     tf)
-find_package(Boost REQUIRED COMPONENTS chrono)
+    
+# find_package(Boost REQUIRED COMPONENTS chrono)
 
 ###################################
 ## catkin specific configuration ##
@@ -24,8 +24,8 @@ find_package(Boost REQUIRED COMPONENTS chrono)
 
 catkin_package(
   INCLUDE_DIRS include
-#   LIBRARIES scout_base
+  LIBRARIES scout_base
-  CATKIN_DEPENDS diagnostic_updater hardware_interface scout_msgs roscpp sensor_msgs
+  CATKIN_DEPENDS hardware_interface scout_msgs roscpp sensor_msgs
 #   DEPENDS Boost
 )
 
@@ -43,11 +43,12 @@ include_directories(
 # add scout sdk
 add_subdirectory(src/scout_sdk)
 
-add_executable(scout_base_node src/scout_base_node.cpp src/scout_messenger.cpp)
+add_library(scout_messenger STATIC src/scout_messenger.cpp)
-target_link_libraries(scout_base_node scout_base ${catkin_LIBRARIES})
+target_link_libraries(scout_messenger scoutbase ${catkin_LIBRARIES})
+set_property(TARGET scout_messenger PROPERTY POSITION_INDEPENDENT_CODE ON)
 
-# add_executable(keybord src/keybord.cpp)
+add_executable(scout_base_node src/scout_base_node.cpp)
-# target_link_libraries(keybord ${catkin_LIBRARIES})
+target_link_libraries(scout_base_node scoutbase scout_messenger ${catkin_LIBRARIES})
 
 #############
 ## Install ##

scout_base/include/scout_base/scout_messenger.hpp

@@ -11,13 +11,12 @@
 #define SCOUT_MESSENGER_HPP
 
 #include <string>
-#include <boost/thread.hpp>
 
 #include <ros/ros.h>
-#include <sensor_msgs/JointState.h>
-#include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
+#include <tf/transform_broadcaster.h>
 
+#include "scout_msgs/ScoutLightCmd.h"
 #include "scout/scout_base.hpp"
 
 namespace wescore
@@ -25,25 +24,32 @@ namespace wescore
 class ScoutROSMessenger
 {
 public:
-    ScoutROSMessenger(ScoutBase &scout, ros::NodeHandle nh);
+    explicit ScoutROSMessenger(ros::NodeHandle nh);
+    ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle nh);
 
-    int control_rate_;
     std::string odom_frame_;
     std::string base_frame_;
 
+    bool simulated_robot_;
+    int sim_control_rate_;
+
     void SetupSubscription();
     void PublishStateToROS();
+    void PublishSimStateToROS(double linear, double angular);
+
+    void GetCurrentMotionCmdForSim(double &linear, double &angular);
 
 private:
-    ScoutBase &scout_;
+    ScoutBase *scout_;
     ros::NodeHandle nh_;
-    int32_t cmd_timeout_counter_ = 0;
 
     std::mutex twist_mutex_;
     geometry_msgs::Twist current_twist_;
 
     ros::Publisher odom_publisher_;
-    ros::Subscriber cmd_subscriber_;
+    ros::Publisher status_publisher_;
+    ros::Subscriber motion_cmd_subscriber_;
+    ros::Subscriber light_cmd_subscriber_;
     tf::TransformBroadcaster tf_broadcaster_;
 
 private:
@@ -58,7 +64,9 @@ private:
     ros::Time current_time_;
 
     void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg);
+    void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg);
+    void PublishOdometryToROS(double linear, double angular, double dt);
 };
-} // namespace scout
+} // namespace wescore
 
 #endif /* SCOUT_MESSENGER_HPP */

scout_base/launch/scout_base.launch

@@ -5,6 +5,6 @@
         <param name="odom_frame" type="string" value="odom" />
         <param name="base_frame" type="string" value="base_footprint" />
 
-        <param name="control_rate" value="10" />
+        <param name="simulated_robot" type="bool" value="false" />
     </node>
 </launch>

scout_base/launch/scout_base_sim.launch

@@ -0,0 +1,14 @@
+<launch>
+
+    <node name="scout_odom" pkg="scout_vrep_sim" type="scout_base_sim" output="screen">
+        <param name="odom_frame" type="string" value="odom" />
+        <param name="base_frame" type="string" value="base_link" />
+        <param name="simulated_robot" type="bool" value="true" />
+        <param name="sim_control_rate" value="50" />
+    </node>
+
+    <!-- Differential controller parameters and basic localization -->
+    <!--
+    <include file="$(find scout_control)/launch/control.launch" />
+    -->
+</launch>

scout_base/package.xml

@@ -4,23 +4,17 @@
   <version>0.3.3</version>
   <description>AgileX Scout robot driver</description>
 
-  <author email="mpurvis@clearpathrobotics.com">Mike Purvis</author>
+  <author email="ruixiang.du@westonrobot.com">Ruixiang Du</author>
-  <author email="paul@bovbel.com">Paul Bovbel</author>
+  <maintainer email="info@westonrobot.com">WestonRobot</maintainer>
-
-  <maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
-  <maintainer email="tbaltovski@clearpathrobotics.com">Tony Baltovski</maintainer>
-
 
   <license>BSD</license>
 
-  <url type="website">http://ros.org/wiki/husky_base</url>
+  <url type="website">TODO</url>
-  <url type="bugtracker">https://github.com/husky/husky_robot/issues</url>
+  <url type="bugtracker">TODO</url>
-  <url type="repository">https://github.com/husky/husky_robot</url>
+  <url type="repository">TODO</url>
 
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>controller_manager</build_depend>
-  <build_depend>diagnostic_updater</build_depend>
-  <build_depend>diagnostic_msgs</build_depend>
   <build_depend>hardware_interface</build_depend>
   <build_depend>scout_msgs</build_depend>
   <build_depend>roscpp</build_depend>
@@ -29,9 +23,6 @@
   <build_depend>sensor_msgs</build_depend>
 
   <run_depend>controller_manager</run_depend>
-  <run_depend>diagnostic_updater</run_depend>
-  <run_depend>diagnostic_msgs</run_depend>
-  <run_depend>diagnostic_aggregator</run_depend>
   <run_depend>diff_drive_controller</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>hardware_interface</run_depend>

scout_base/src/scout_base_node.cpp

@@ -19,12 +19,13 @@ int main(int argc, char **argv)
 
     // instantiate a robot
     ScoutBase robot;
-    ScoutROSMessenger messenger(robot, node);
+    ScoutROSMessenger messenger(&robot, node);
 
     std::string scout_can_port;
-    private_node.param<std::string>("port_name_", scout_can_port, std::string("can1"));
+    private_node.param<std::string>("port_name", scout_can_port, std::string("can1"));
-    private_node.param<std::string>("odom_frame_", messenger.odom_frame_, std::string("odom"));
+    private_node.param<std::string>("odom_frame", messenger.odom_frame_, std::string("odom"));
-    private_node.param<std::string>("base_frame_", messenger.base_frame_, std::string("base_footprint"));
+    private_node.param<std::string>("base_frame", messenger.base_frame_, std::string("base_footprint"));
+    private_node.param<bool>("simulated_robot", messenger.simulated_robot_, false);
 
     // connect to scout and setup ROS subscription
     robot.ConnectCANBus(scout_can_port);

scout_base/src/scout_messenger.cpp

@@ -9,9 +9,15 @@
 
 #include "scout_base/scout_messenger.hpp"
 
+#include "scout_msgs/ScoutStatus.h"
+
 namespace wescore
 {
-ScoutROSMessenger::ScoutROSMessenger(ScoutBase &scout, ros::NodeHandle nh) : scout_(scout), nh_(nh)
+ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle nh) : scout_(nullptr), nh_(nh)
+{
+}
+
+ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle nh) : scout_(scout), nh_(nh)
 {
 }
 
@@ -19,14 +25,102 @@ void ScoutROSMessenger::SetupSubscription()
 {
     // odometry publisher
     odom_publisher_ = nh_.advertise<nav_msgs::Odometry>(odom_frame_, 50);
+    status_publisher_ = nh_.advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
 
     // cmd subscriber
-    cmd_subscriber_ = nh_.subscribe<geometry_msgs::Twist>("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); //不启用平滑包则订阅“cmd_vel”
+    motion_cmd_subscriber_ = nh_.subscribe<geometry_msgs::Twist>("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); //不启用平滑包则订阅“cmd_vel”
+    light_cmd_subscriber_ = nh_.subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
 }
 
 void ScoutROSMessenger::TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg)
 {
-    scout_.SetMotionCommand(msg->linear.x, msg->angular.z);
+    if (!simulated_robot_)
+    {
+        scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
+    }
+    else
+    {
+        std::lock_guard<std::mutex> guard(twist_mutex_);
+        current_twist_ = *msg.get();
+    }
+}
+
+void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, double &angular)
+{
+    std::lock_guard<std::mutex> guard(twist_mutex_);
+    linear = current_twist_.linear.x;
+    angular = current_twist_.angular.z;
+}
+
+void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg)
+{
+    if (!simulated_robot_)
+    {
+        if (msg->enable_cmd_light_control)
+        {
+            ScoutLightCmd cmd;
+
+            switch (msg->front_mode)
+            {
+            case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
+            {
+                cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
+                break;
+            }
+            case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
+            {
+                cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
+                break;
+            }
+            case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
+            {
+                cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
+                break;
+            }
+            case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
+            {
+                cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
+                cmd.front_custom_value = msg->front_custom_value;
+                break;
+            }
+            }
+
+            switch (msg->rear_mode)
+            {
+            case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
+            {
+                cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
+                break;
+            }
+            case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
+            {
+                cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
+                break;
+            }
+            case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
+            {
+                cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
+                break;
+            }
+            case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
+            {
+                cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
+                cmd.rear_custom_value = msg->rear_custom_value;
+                break;
+            }
+            }
+
+            scout_->SetLightCommand(cmd);
+        }
+        else
+        {
+            scout_->DisableLightCmdControl();
+        }
+    }
+    else
+    {
+        std::cout << "simulated robot received light control cmd" << std::endl;
+    }
 }
 
 void ScoutROSMessenger::PublishStateToROS()
@@ -42,9 +136,85 @@ void ScoutROSMessenger::PublishStateToROS()
         return;
     }
 
-    auto state = scout_.GetScoutState();
+    auto state = scout_->GetScoutState();
-    double linear = state.linear_velocity;
+
-    double angular = state.angular_velocity;
+    // publish scout state message
+    scout_msgs::ScoutStatus status_msg;
+
+    status_msg.header.stamp = current_time_;
+
+    status_msg.linear_velocity = state.linear_velocity;
+    status_msg.angular_velocity = state.angular_velocity;
+
+    status_msg.base_state = state.base_state;
+    status_msg.control_mode = state.control_mode;
+    status_msg.fault_code = state.fault_code;
+    status_msg.battery_voltage = state.battery_voltage;
+
+    for (int i = 0; i < 4; ++i)
+    {
+        status_msg.motor_states[i].current = state.motor_states[i].current;
+        status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
+        status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
+    }
+
+    status_msg.light_control_enabled = state.light_control_enabled;
+    status_msg.front_light_state.mode = state.front_light_state.mode;
+    status_msg.front_light_state.custom_value = state.front_light_state.custom_value;
+    status_msg.rear_light_state.mode = state.rear_light_state.mode;
+    status_msg.rear_light_state.custom_value = state.front_light_state.custom_value;
+
+    status_publisher_.publish(status_msg);
+
+    // publish odometry and tf
+    PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt);
+
+    // record time for next integration
+    last_time_ = current_time_;
+}
+
+void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
+{
+    current_time_ = ros::Time::now();
+    double dt = 1.0 / sim_control_rate_;
+
+    // publish scout state message
+    scout_msgs::ScoutStatus status_msg;
+
+    status_msg.header.stamp = current_time_;
+
+    status_msg.linear_velocity = linear;
+    status_msg.angular_velocity = angular;
+
+    status_msg.base_state = 0x00;
+    status_msg.control_mode = 0x01;
+    status_msg.fault_code = 0x00;
+    status_msg.battery_voltage = 29.5;
+
+    // for (int i = 0; i < 4; ++i)
+    // {
+    //     status_msg.motor_states[i].current = state.motor_states[i].current;
+    //     status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
+    //     status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
+    // }
+
+    status_msg.light_control_enabled = false;
+    // status_msg.front_light_state.mode = state.front_light_state.mode;
+    // status_msg.front_light_state.custom_value = state.front_light_state.custom_value;
+    // status_msg.rear_light_state.mode = state.rear_light_state.mode;
+    // status_msg.rear_light_state.custom_value = state.front_light_state.custom_value;
+
+    status_publisher_.publish(status_msg);
+
+    // publish odometry and tf
+    PublishOdometryToROS(linear, angular, dt);
+}
+
+void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt)
+{
+    // perform numerical integration to get an estimation of pose
+    linear_speed_ = linear;
+    angular_speed_ = angular;
 
     double d_x = linear_speed_ * std::cos(theta_) * dt;
     double d_y = linear_speed_ * std::sin(theta_) * dt;
@@ -54,10 +224,9 @@ void ScoutROSMessenger::PublishStateToROS()
     position_y_ += d_y;
     theta_ += d_theta;
 
-    //since all odometry is 6DOF we'll need a quaternion created from yaw
     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_);
 
-    //first, we'll publish the transform over tf
+    // publish tf transformation
     geometry_msgs::TransformStamped tf_msg;
     tf_msg.header.stamp = current_time_;
     tf_msg.header.frame_id = odom_frame_;
@@ -70,7 +239,7 @@ void ScoutROSMessenger::PublishStateToROS()
 
     tf_broadcaster_.sendTransform(tf_msg);
 
-    //next, we'll publish the odometry message over ROS
+    // publish odometry and tf messages
     nav_msgs::Odometry odom_msg;
     odom_msg.header.stamp = current_time_;
     odom_msg.header.frame_id = odom_frame_;
@@ -85,9 +254,6 @@ void ScoutROSMessenger::PublishStateToROS()
     odom_msg.twist.twist.linear.y = 0.0;
     odom_msg.twist.twist.angular.z = angular_speed_;
 
-    //publish the message
     odom_publisher_.publish(odom_msg);
-
-    last_time_ = current_time_;
 }
-} // namespace scout
+} // namespace wescore

scout_base/src/scout_sdk/CHANGELOG.md

@@ -0,0 +1,15 @@
+# Changelog for scout_sdk
+
+## 0.2 (2019-06-14)
+------------------
+* Deprecated initial serial interface support (new one under development)
+* Added full CAN support
+* Improved multi-threading implementation
+* Contributors: Ruixiang Du
+  
+## 0.1 (2019-05-07)
+------------------
+
+* Added basic serial communication support 
+* Provided C++ interface, ROS/LCM demo
+* Contributors: Ruixiang Du

scout_base/src/scout_sdk/CMakeLists.txt

@@ -1,21 +1,50 @@
-cmake_minimum_required(VERSION 3.0.0)
+cmake_minimum_required(VERSION 3.1.0)
 project(scout_sdk)
 
 # generate symbols for IDE indexer
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 set(RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/devel)
 
+## Set compiler to use c++ 11 features
+set(CMAKE_CXX_STANDARD 11)
+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(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
 set(ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
 
+## Use GNUInstallDirs to install libraries into correct
+# locations on all platforms.
+include(GNUInstallDirs)
+
 ## Chosse build type
-# set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_BUILD_TYPE Release)
 # set(CMAKE_BUILD_TYPE Debug)
 
+set(default_build_type "Release")
+if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+  message(STATUS "Setting build type to '${default_build_type}' as none was specified.")
+  set(CMAKE_BUILD_TYPE "${default_build_type}" CACHE
+      STRING "Choose the type of build." FORCE)
+  # Set the possible values of build type for cmake-gui
+  set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS
+    "Debug" "Release" "MinSizeRel" "RelWithDebInfo")
+endif()
+
 ## Optionally built modules: ON/OFF
 set(BUILD_TESTS OFF)
+set(BUILD_MONITOR ON)
+
+# Disable monitor if ncurses library is not found
+set(CURSES_NEED_NCURSES TRUE)
+find_package(Curses QUIET)
+
+if(BUILD_MONITOR AND NOT CURSES_FOUND)
+    set(BUILD_MONITOR OFF)
+    message(STATUS "Monitor app will not be built due to missing ncurses library, try: sudo apt install libncurses5-dev")
+endif()
 
 ## Add source directories
 add_subdirectory(src)

scout_base/src/scout_sdk/README.md

@@ -1,20 +1,23 @@
 # SDK for AgileX Scout Mobile Base
 
+Copyright (c) 2019 [WestonRobot](https://www.westonrobot.com/)
+
 ## Introduction
 
 This software packages provides a C++ interface to communicate with the Scout mobile base, for sending commands to the robot and acquiring latest robot state.
 
-Generally, you only need to instantiate an object of "class ScoutBase", then use the object to programmatically control the robot. Conceptually, class ScoutBase manages two background threads, one to process CAN messages of the robot state and accordingly update state variables in the ScoutState data structre, and the other to maintain a 50Hz loop and send latest command to the robot base. User can check the robot state or set control commands, as well as other tasks, in the main thread. 
+Generally, you only need to instantiate an object of "class ScoutBase", then use the object to programmatically control the robot. Internally, class ScoutBase manages two background threads, one to process CAN messages of the robot state and accordingly update state variables in the ScoutState data structre, and the other to maintain a 50Hz loop and send latest command to the robot base. User can iteratively perform tasks in the main thread and check the robot state or set control commands. 
 
-See "src/demo/demo_scout_can.cpp" for an example.
+Refer to "src/demo/demo_scout_can.cpp" for an example.
 
 ## Package Structure
 
-* scout: interface to send command to robot and receive robot state
+* demo: demo to illustrate how to use the SDK
-* driver: manages raw data communication with robot
+* monitor: a TUI application to monitor states of Scout
+* sdk_core/scout_base: interface to send command to robot and receive robot state
+* sdk_core/async_io: manages raw data communication with robot
 * third_party
     - asio: asynchronous IO management (serial and CAN)
-    - stopwatch: timing control
     - googletest: for unit tests only (not required otherwise)
 
 ## Hardware Interface
@@ -24,6 +27,34 @@ See "src/demo/demo_scout_can.cpp" for an example.
 
 A easy and low-cost option to use the CAN interface would be using a Raspberry Pi or Beaglebone board with CAN Hat/Cape. The SDK can compile on both x86 and ARM platforms. Then you can use whatever interface you prefer (serial, USB, Ethernet etc.) for the communication between the single-board computer and your main PC.
 
+* Setup CAN-To-USB adapter 
+ 
+The intructions work for stm32f0-based adapter with [candleLight](https://github.com/HubertD/candleLight_fw) firmware on a host computer running Linux. (Refer to limitations listed at the bottom for more details.)
+
+1. Enable gs_usb kernel module
+    ```
+    $ sudo modprobe gs_usb
+    ```
+2. Bringup can device
+   ```
+   $ sudo ip link set can0 up type can bitrate 500000
+   ```
+3. If no error occured during the previous steps, you should be able to see the can device now by using command
+   ```
+   $ ifconfig -a
+   ```
+4. Install and use can-utils to test the hardware
+    ```
+    $ sudo apt install can-utils
+    ```
+5. Testing command
+    ```
+    # receiving data from can0
+    $ candump can0
+    # send data to can0
+    $ cansend can0 001#1122334455667788
+    ```
+
 ## Build SDK
 
 Install compile tools
@@ -32,6 +63,12 @@ Install compile tools
 $ sudo apt install build-essential cmake
 ```
 
+If you want to build the monitor, install libncurses
+
+```
+$ sudo apt install libncurses5-dev
+```
+
 Configure and build
 
 ```
@@ -56,6 +93,11 @@ $ make
             device interrupt 43
     ```
     
-    If you use a CAN-to-USB adapter, make sure it supports slcan or can be brought up as a native CAN device (for example, CANable https://www.canable.io/). Some adapters may use a custom-defined protocol and appear as a serial device in Linux. In such case, you will have to translate the bytes between CAN and UART by yourself. It would be difficult for us to provide support for them since not all manufactures define this protocol in the same way.
+    If you use a CAN-to-USB adapter, make sure it supports slcan or can be brought up as a native CAN device (for example, CANable https://www.canable.io/). Some adapters may use a custom-defined protocol and appear as a serial device in Linux. In such case, you will have to translate the byte stream between CAN and UART by yourself. It would be difficult for us to provide support for them since not all manufactures define this protocol in the same way.
 
-2. Release v0.1 of this SDK provided a serial interface to talk with the robot. Front/rear light on the robot cannot be controlled and only a small subset of all robot states can be acquired throught that interface. Full support of the serial interface is still under development and requires additional work in both the SDK and firmware.
+2. Release v0.1 of this SDK provided a serial interface to talk with the robot. Front/rear light on the robot cannot be controlled and only a small subset of all robot states can be acquired through that interface. Full support of the serial interface is still under development and requires additional work on both the SDK and firmware sides.
+
+## Reference
+
+* [Candlelight firmware](https://wiki.linklayer.com/index.php/CandleLightFirmware)
+* [CAN command reference in Linux](https://wiki.rdu.im/_pages/Notes/Embedded-System/Linux/can-bus-in-linux.html)

scout_base/src/scout_sdk/docs/beaglebone_black_can_cape.md

@@ -0,0 +1,23 @@
+# Setup CAN on Beaglebone Black
+
+CAN/RS485 Cape: https://item.taobao.com/item.htm?spm=a1z09.2.0.0.68b02e8d02l2uT&id=42637485181&_u=a4mg52ma183
+
+* Install latest Debian image from offical site
+* Build and install overlays
+```
+$ sudo apt install device-tree-compiler
+$ git clone https://github.com/beagleboard/bb.org-overlays
+$ cd ./bb.org-overlays/
+$ ./install.sh
+```
+* Edit /boot/uEnv.txt
+```
+# 1. Add the following overlay
+###Custom Cape
+dtb_overlay=/lib/firmware/BB-CAN1-00A0.dtbo
+
+# 2. Disable auto loading of virutal capes (uncomment the two lines)
+disable_uboot_overlay_video=1
+disable_uboot_overlay_audio=1
+```
+* Reboot the system

scout_base/src/scout_sdk/docs/beaglebone_ros_install.md

@@ -0,0 +1,51 @@
+```
+$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu stretch main" > /etc/apt/sources.list.d/ros-latest.list'
+$ wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -O - | sudo apt-key add -
+$ sudo apt-get update
+```
+
+```
+$ sudo apt-get install -y python-pip python-setuptools python-yaml python-distribute python-docutils python-dateutil python-six
+$ sudo pip install rosdep rosinstall_generator wstool rosinstall
+$ sudo apt-get install -y \
+     libconsole-bridge-dev liblz4-dev checkinstall cmake \
+     python-empy python-nose libbz2-dev \
+     libboost-test-dev libboost-dev  libboost-program-options-dev \
+     libboost-regex-dev libboost-signals-dev \
+     libtinyxml-dev libboost-filesystem-dev libxml2-dev \
+     libgtest-dev libpoco-dev
+```
+
+```
+$ sudo rosdep init
+$ rosdep update
+```
+
+```
+$ rosinstall_generator ros_comm --rosdistro melodic --deps --tar > melodic-ros_comm.rosinstall
+$ wstool init -j1 src melodic-ros_comm.rosinstall
+```
+
+```
+$ # rosdep install --from-paths src --ignore-src --rosdistro melodic -y
+$ sudo ./src/catkin/bin/catkin_make_isolated --install --install-space /opt/ros/melodic -DCMAKE_BUILD_TYPE=Release
+```
+
+```
+$ source /opt/ros/melodic/setup.bash
+```
+
+Additional runtime dependencies:
+
+```
+$ sudo apt install -y python-defusedxml python-netifaces
+```
+
+```
+$ sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys F42ED6FBAB17C654
+```
+
+Reference:
+
+* [1] https://machinekoder.com/ros-with-debian-stretch-on-the-beaglebone-black-green-blue/
+* [2] http://wiki.ros.org/melodic/Installation/Source

scout_base/src/scout_sdk/docs/check_endianness.md

@@ -0,0 +1,36 @@
+On a Big Endian-System (Solaris on SPARC)
+
+```
+$ echo -n I | od -to2 | head -n1 | cut -f2 -d" " | cut -c6 
+0
+```
+On a little endian system (Linux on x86)
+```
+$ echo -n I | od -to2 | head -n1 | cut -f2 -d" " | cut -c6 
+1
+```
+The solution above is clever and works great for Linux *86 and Solaris Sparc.
+
+I needed a shell-only (no Perl) solution that also worked on AIX/Power and HPUX/Itanium. Unfortunately the last two don't play nice: AIX reports "6" and HPUX gives an empty line.
+
+Using your solution, I was able to craft something that worked on all these Unix systems:
+```
+$ echo I | tr -d [:space:] | od -to2 | head -n1 | awk '{print $2}' | cut -c6
+```
+Regarding the Python solution someone posted, it does not work in Jython because the JVM treats everything as Big. If anyone can get it to work in Jython, please post!
+
+Also, I found this, which explains the endianness of various platforms. Some hardware can operate in either mode depending on what the O/S selects: http://labs.hoffmanlabs.com/node/544
+
+If you're going to use awk this line can be simplified to:
+```
+echo -n I | od -to2 | awk '{ print substr($2,6,1); exit}'
+```
+For small Linux boxes that don't have 'od' (say OpenWrt) then try 'hexdump':
+```
+echo -n I | hexdump -o | awk '{ print substr($2,6,1); exit}'
+```
+
+Reference: 
+
+* [1] https://serverfault.com/questions/163487/how-to-tell-if-a-linux-system-is-big-endian-or-little-endian
+* [2] https://wiki.rdu.im/_pages/Knowledge-Base/Computing/Computing.html

scout_base/src/scout_sdk/docs/commands.md

@@ -0,0 +1,55 @@
+# Commands
+
+## Create gif animation using "imagemagick"
+
+```
+$ convert -delay 120 -loop 0 *.png animated.gif
+```
+
+## Get statistics of the code base
+
+```
+$ sudo apt install cloc
+$ cd ~/Workspace/librav/src
+$ cloc --exclude-dir=cmake,lcmtypes,third_party .
+```
+
+## Create a pair of VSP’s
+
+```
+$ socat -d -d pty,raw,echo=0 pty,raw,echo=0
+```
+
+```
+$ cat nmea_test.txt > /dev/pts/6
+```
+
+## git subtree
+
+Adding the sub-project as a remote
+```
+$ git remote add -f [remote-name] [remote-url]
+$ git subtree add --prefix [sub-project-name] [remote-name] [branch-name] --squash
+```
+
+Update the sub-project
+```
+$ git fetch [remote-name] [branch-name] 
+$ git subtree pull --prefix [sub-project-name] [remote-name] [branch-name] --squash
+```
+
+Push to remote
+```
+$ git subtree push --prefix=[sub-project-name] [remote-name] [branch-name] 
+```
+
+Firmware branch update
+```
+$ git fetch fw_origin pios_pixcar
+$ git subtree pull --prefix firmware fw_origin pios_pixcar --squash
+```
+
+## Reference
+
+* [Git subtree: the alternative to Git submodule](https://www.atlassian.com/blog/git/alternatives-to-git-submodule-git-subtree)
+* [Virtual serial port: socat](https://justcheckingonall.wordpress.com/2009/06/09/howto-vsp-socat/)

scout_base/src/scout_sdk/docs/jetson_tx2_can.md

@@ -0,0 +1,12 @@
+# Enable CAN on Jetson TX2
+
+```
+$ sudo modprobe can
+$ sudo modprobe mttcan
+$ sudo ip link set can0 type can bitrate 500000
+$ sudo ip link set up can0
+```
+
+Reference:
+
+* https://devtalk.nvidia.com/default/topic/1006762/jetson-tx2/how-can-i-use-can-bus-in-tx2-/post/5166583/#5166583

scout_base/src/scout_sdk/docs/release_checklist.md

@@ -0,0 +1 @@
+* Disable tests to keep minimal dependency by default

scout_base/src/scout_sdk/docs/test_can_msg.txt

@@ -0,0 +1,7 @@
+  can1  131   [8]  00 00 00 00 00 00 E4 1E
+  can1  141   [8]  00 00 00 02 0E 00 E3 3D
+  can1  151   [8]  01 00 00 EF 00 3C E5 6B
+  can1  200   [8]  00 00 00 00 00 00 EA F4
+  can1  201   [8]  00 00 00 00 00 00 EA F5
+  can1  202   [8]  00 00 00 00 00 00 EA F6
+  can1  203   [8]  00 00 00 00 00 00 EA F7

scout_base/src/scout_sdk/scripts/remote_ws/beaglebone_black.code-workspace

@@ -0,0 +1,6 @@
+{
+    "folders": [{
+        "uri": "sftp://rdu:rdu3317*@192.168.7.2/home/rdu/Workspace/scout_sdk",
+        "name": "Scout SDK Workspace"
+    }]
+}

scout_base/src/scout_sdk/src/CMakeLists.txt

@@ -1,9 +1,8 @@
 # Add source directories
 add_subdirectory(demo)
-add_subdirectory(scout)
+add_subdirectory(sdk_core)
-add_subdirectory(driver)
 add_subdirectory(third_party)
 
-if(BUILD_TESTS)
+if(BUILD_MONITOR)
-    add_subdirectory(unit_tests)
+add_subdirectory(monitor)
 endif()

scout_base/src/scout_sdk/src/demo/CMakeLists.txt

@@ -3,4 +3,4 @@
 
 # tests
 add_executable(demo_scout_can demo_scout_can.cpp)
-target_link_libraries(demo_scout_can scout_base)
+target_link_libraries(demo_scout_can scoutbase)

scout_base/src/scout_sdk/src/driver/CMakeLists.txt

@@ -1,10 +0,0 @@
-cmake_minimum_required(VERSION 3.0.0)
-project(driver)
-
-## Put all binary files into /bin and libraries into /lib
-#set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
-set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
-set(ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
-
-## Add sub source directories
-add_subdirectory(async_io)

scout_base/src/scout_sdk/src/driver/async_io/CMakeLists.txt

@@ -1,20 +0,0 @@
-# Dependency libraries
-# find_package(LIB_NAME REQUIRED)
-
-# Add libraries
-set(ASYNC_IO_LIB_SRC
-    src/async_serial.cpp
-    src/async_can.cpp
-)
-add_library(asyncio STATIC ${ASYNC_IO_LIB_SRC})
-target_link_libraries(asyncio asio pthread)
-target_compile_definitions(asyncio PUBLIC "-DASIO_ENABLE_OLD_SERVICES -DASIO_HAS_POSIX_STREAM_DESCRIPTOR")
-target_include_directories(asyncio PUBLIC
-	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-	$<INSTALL_INTERFACE:include>
-	PRIVATE src)
-
-# Add executables
-if(BUILD_TESTS)
-	add_subdirectory(tests)
-endif()

scout_base/src/scout_sdk/src/monitor/CMakeLists.txt

@@ -0,0 +1,24 @@
+# Dependency libraries
+set(CURSES_NEED_NCURSES TRUE)
+find_package(Curses REQUIRED)
+
+# Add libraries
+set(SCOUT_MONITOR_SRC
+    src/nshapes.cpp
+    src/ncolors.cpp
+    src/scout_monitor.cpp
+)
+add_library(monitor STATIC ${SCOUT_MONITOR_SRC})
+target_link_libraries(monitor scoutbase ${CURSES_LIBRARIES})
+target_include_directories(monitor PUBLIC
+	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${CURSES_INCLUDE_DIR}>
+	$<INSTALL_INTERFACE:include>
+	PRIVATE src)
+
+add_subdirectory(app)
+
+# Add executables
+if(BUILD_TESTS)
+	add_subdirectory(tests)
+endif()

scout_base/src/scout_sdk/src/monitor/app/CMakeLists.txt

@@ -0,0 +1,3 @@
+# Add executables
+add_executable(scout_monitor app_scout_monitor.cpp)
+target_link_libraries(scout_monitor monitor)

scout_base/src/scout_sdk/src/monitor/app/app_scout_monitor.cpp

@@ -0,0 +1,31 @@
+#include <signal.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <iostream>
+
+#include "monitor/scout_monitor.hpp"
+
+using namespace wescore;
+
+int main(int argc, char **argv)
+{
+    std::string device_name;
+
+    if (argc == 2)
+    {
+        device_name = {argv[1]};
+        std::cout << "Specified device: " << device_name << std::endl;
+    }
+    else
+    {
+        std::cout << "Usage: app_scout_monitor <interface>" << std::endl
+                  << "Example: ./app_scout_monitor can1" << std::endl;
+        return -1;
+    }
+
+    ScoutMonitor monitor;
+    monitor.Run(device_name);
+
+    return 0;
+}

scout_base/src/scout_sdk/src/monitor/include/monitor/ncolors.hpp

@@ -0,0 +1,67 @@
+/* 
+ * ncolors.hpp
+ * 
+ * Created on: Jun 20, 2019 06:22
+ * Description: 
+ * 
+ * Original source: https://www.linuxjournal.com/content/about-ncurses-colors-0
+ * This copy is based on the original implementation, modified and 
+ *  maintained by Ruixiang.
+ * 
+ * Copyright (c) 2018 Jim Hall
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#ifndef NCOLORS_HPP
+#define NCOLORS_HPP
+
+#include <ncurses.h>
+
+namespace wescore
+{
+struct NColors
+{
+    enum BackgroundColor
+    {
+        BLACK = 0,
+        BLUE,
+        GREEN,
+        CYAN,
+        RED,
+        MAGENTA,
+        YELLOW,
+        WHITE
+    };
+
+    enum ForegroundColor
+    {
+        /*
+        BLACK = 0,
+        BLUE,
+        GREEN,
+        CYAN,
+        RED,
+        MAGENTA,
+        YELLOW,
+        WHITE,*/
+        BRIGHT_BLACK = 8,
+        BRIGHT_BLUE,
+        BRIGHT_GREEN,
+        BRIGHT_CYAN,
+        BRIGHT_RED,
+        BRIGHT_MAGENTA,
+        BRIGHT_YELLOW,
+        BRIGHT_WHITE
+    };
+
+    static void InitColors();
+
+    static void SetColor(int fg, int bg = BLACK);
+    static void UnsetColor(int fg, int bg = BLACK);
+    
+    static void WSetColor(WINDOW *win, int fg, int bg = BLACK);
+    static void WUnsetColor(WINDOW *win, int fg, int bg = BLACK);
+};
+} // namespace wescore
+
+#endif /* NCOLORS_HPP */

scout_base/src/scout_sdk/src/monitor/include/monitor/nshapes.hpp

@@ -0,0 +1,24 @@
+/* 
+ * nshapes.hpp
+ * 
+ * Created on: Jun 20, 2019 06:21
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#ifndef NSHAPES_HPP
+#define NSHAPES_HPP
+
+#include <ncurses.h>
+
+namespace wescore
+{
+struct NShapes
+{
+    static void DrawRectangle(int tl_y, int tl_x, int br_y, int br_x);
+    static void WDrawRectangle(WINDOW *win, int tl_y, int tl_x, int br_y, int br_x);
+};
+} // namespace wescore
+
+#endif /* NSHAPES_HPP */

scout_base/src/scout_sdk/src/monitor/include/monitor/scout_monitor.hpp

@@ -0,0 +1,79 @@
+/* 
+ * scout_monitor.hpp
+ * 
+ * Created on: Jun 12, 2019 01:19
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#ifndef SCOUT_MONITOR_HPP
+#define SCOUT_MONITOR_HPP
+
+#include "scout/scout_base.hpp"
+#include "scout/scout_state.hpp"
+
+#include <ncurses.h>
+
+namespace wescore
+{
+class ScoutMonitor
+{
+public:
+    ScoutMonitor();
+    ~ScoutMonitor();
+
+    void Run(std::string device_name = "");
+    void Terminate() { keep_running_ = false; }
+
+private:
+    bool keep_running_ = true;
+    bool test_mode_ = true;
+
+    int term_sx_ = -1;
+    int term_sy_ = -1;
+
+    WINDOW *body_info_win_;
+    int bi_win_sx_;
+    int bi_win_sy_;
+    int bi_origin_x_;
+    int bi_origin_y_;
+
+    WINDOW *system_info_win_;
+    int si_win_sx_;
+    int si_win_sy_;
+    int si_origin_x_;
+    int si_origin_y_;
+
+    WINDOW *scout_cmd_win_;
+
+    ScoutBase scout_base_;
+    ScoutState scout_state_;
+
+    const int linear_axis_length_ = 5;
+    const int angular_axis_length_ = 5;
+
+    const int vehicle_fp_offset_x_ = 9;
+    const int vehicle_fp_offset_y_ = 9;
+
+    bool resizing_detected_;
+
+    void UpdateAll();
+    void ClearAll();
+
+    void CalcDimensions();
+    void HandleResizing();
+
+    void SetTestStateData();
+    void ShowVehicleState(int y, int x);
+    void ShowStatusItemName(int y, int x, std::string name);
+    void ShowFault(int y, int x, bool no_fault);
+    void ShowMotorInfo(int y, int x, double cur, int rpm, int temp, bool is_right);
+
+    void UpdateScoutBodyInfo();
+    void UpdateScoutSystemInfo();
+    void UpdateScoutCmdWindow();
+};
+} // namespace wescore
+
+#endif /* SCOUT_MONITOR_HPP */

scout_base/src/scout_sdk/src/monitor/src/ncolors.cpp

@@ -0,0 +1,114 @@
+/* 
+ * ncolors.cpp
+ * 
+ * Created on: Jun 20, 2019 06:22
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#include "monitor/ncolors.hpp"
+
+#include <iostream>
+
+namespace
+{
+int IsBold(int fg)
+{
+    /* return the intensity bit */
+
+    int i;
+
+    i = 1 << 3;
+    return (i & fg);
+}
+
+int ColorNum(int fg, int bg)
+{
+    int B, bbb, ffff;
+
+    B = 1 << 7;
+    bbb = (7 & bg) << 4;
+    ffff = 7 & fg;
+
+    return (B | bbb | ffff);
+}
+
+short CursorColor(int fg)
+{
+    switch (7 & fg)
+    {       /* RGB */
+    case 0: /* 000 */
+        return (COLOR_BLACK);
+    case 1: /* 001 */
+        return (COLOR_BLUE);
+    case 2: /* 010 */
+        return (COLOR_GREEN);
+    case 3: /* 011 */
+        return (COLOR_CYAN);
+    case 4: /* 100 */
+        return (COLOR_RED);
+    case 5: /* 101 */
+        return (COLOR_MAGENTA);
+    case 6: /* 110 */
+        return (COLOR_YELLOW);
+    case 7: /* 111 */
+        return (COLOR_WHITE);
+    }
+}
+} // namespace
+
+namespace wescore
+{
+void NColors::InitColors()
+{
+    if (has_colors() != FALSE)
+        start_color();
+    else
+        std::cerr << "Your terminal does not support color" << std::endl;
+
+    int fg, bg;
+    int colorpair;
+
+    for (bg = 0; bg <= 7; bg++)
+    {
+        for (fg = 0; fg <= 7; fg++)
+        {
+            colorpair = ColorNum(fg, bg);
+            init_pair(colorpair, CursorColor(fg), CursorColor(bg));
+        }
+    }
+}
+
+void NColors::SetColor(int fg, int bg)
+{
+    // set the color pair (ColorNum) and bold/bright (A_BOLD)
+    attron(COLOR_PAIR(ColorNum(fg, bg)));
+    if (IsBold(fg))
+        attron(A_BOLD);
+}
+
+void NColors::UnsetColor(int fg, int bg)
+{
+    // unset the color pair (ColorNum) and bold/bright (A_BOLD)
+    attroff(COLOR_PAIR(ColorNum(fg, bg)));
+    if (IsBold(fg))
+        attroff(A_BOLD);
+}
+
+void NColors::WSetColor(WINDOW *win, int fg, int bg)
+{
+    // set the color pair (ColorNum) and bold/bright (A_BOLD)
+    wattron(win, COLOR_PAIR(ColorNum(fg, bg)));
+    if (IsBold(fg))
+        wattron(win, A_BOLD);
+}
+
+void NColors::WUnsetColor(WINDOW *win, int fg, int bg)
+{
+    // unset the color pair (ColorNum) and bold/bright (A_BOLD)
+    wattroff(win, COLOR_PAIR(ColorNum(fg, bg)));
+    if (IsBold(fg))
+        wattroff(win, A_BOLD);
+}
+} // namespace wescore

scout_base/src/scout_sdk/src/monitor/src/nshapes.cpp

@@ -0,0 +1,41 @@
+/* 
+ * nshapers.cpp
+ * 
+ * Created on: Jun 20, 2019 06:21
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#include "monitor/nshapes.hpp"
+
+namespace wescore
+{
+void NShapes::DrawRectangle(int tl_y, int tl_x, int br_y, int br_x)
+{
+    for (int i = tl_y; i <= br_y; ++i)
+    {
+        mvprintw(i, tl_x, "|");
+        mvprintw(i, br_x, "|");
+    }
+    for (int i = tl_x; i <= br_x; ++i)
+    {
+        mvprintw(tl_y, i, "-");
+        mvprintw(br_y, i, "-");
+    }
+}
+
+void NShapes::WDrawRectangle(WINDOW *win, int tl_y, int tl_x, int br_y, int br_x)
+{
+    for (int i = tl_y; i <= br_y; ++i)
+    {
+        mvwprintw(win, i, tl_x, "|");
+        mvwprintw(win, i, br_x, "|");
+    }
+    for (int i = tl_x; i <= br_x; ++i)
+    {
+        mvwprintw(win, tl_y, i, "-");
+        mvwprintw(win, br_y, i, "-");
+    }
+}
+} // namespace wescore

scout_base/src/scout_sdk/src/monitor/src/scout_monitor.cpp

@@ -0,0 +1,654 @@
+/* 
+ * scout_monitor.cpp
+ * 
+ * Created on: Jun 12, 2019 01:19
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+/*
+ * Coordinate System:
+ *
+ *		o --------------------> x
+ *		|
+ *		|
+ *		|
+ *		|
+ *		|
+ *		|
+ *		v
+ *		y
+ */
+
+#include "monitor/scout_monitor.hpp"
+
+#include <cmath>
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ratio>
+#include <thread>
+
+#include "scout/scout_can_protocol.h"
+#include "scout/scout_command.hpp"
+#include "monitor/nshapes.hpp"
+#include "monitor/ncolors.hpp"
+
+namespace
+{
+// reference: https://thispointer.com/c-convert-double-to-string-and-manage-precision-scientific-notation/
+std::string ConvertFloatToString(double vel, int digit_num = 3)
+{
+    std::ostringstream streamObj;
+    streamObj << std::fixed;
+    streamObj << std::setprecision(digit_num);
+    streamObj << vel;
+    return streamObj.str();
+}
+
+// source: https://github.com/rxdu/stopwatch
+struct StopWatch
+{
+    using Clock = std::chrono::high_resolution_clock;
+    using time_point = typename Clock::time_point;
+    using duration = typename Clock::duration;
+
+    StopWatch() { tic_point = Clock::now(); };
+
+    time_point tic_point;
+
+    void tic()
+    {
+        tic_point = Clock::now();
+    };
+
+    double toc()
+    {
+        return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count() / 1000000.0;
+    };
+
+    // for different precisions
+    double stoc()
+    {
+        return std::chrono::duration_cast<std::chrono::seconds>(Clock::now() - tic_point).count();
+    };
+
+    double mtoc()
+    {
+        return std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
+    };
+
+    double utoc()
+    {
+        return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
+    };
+
+    double ntoc()
+    {
+        return std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() - tic_point).count();
+    };
+
+    // you have to call tic() before calling this function
+    void sleep_until_ms(int64_t period_ms)
+    {
+        int64_t duration = period_ms - std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
+
+        if (duration > 0)
+            std::this_thread::sleep_for(std::chrono::milliseconds(duration));
+    };
+
+    void sleep_until_us(int64_t period_us)
+    {
+        int64_t duration = period_us - std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
+
+        if (duration > 0)
+            std::this_thread::sleep_for(std::chrono::microseconds(duration));
+    };
+};
+} // namespace
+
+namespace wescore
+{
+ScoutMonitor::ScoutMonitor()
+{
+    // init ncurses
+    initscr();
+    // raw();
+    cbreak();
+    noecho();
+    nonl();
+    curs_set(FALSE);
+    intrflush(stdscr, FALSE);
+    keypad(stdscr, TRUE);
+
+    CalcDimensions();
+
+    // setup sub-windows
+    body_info_win_ = newwin(bi_win_sy_, bi_win_sx_, bi_origin_y_, bi_origin_x_);
+    system_info_win_ = newwin(si_win_sy_, si_win_sx_, si_origin_y_, si_origin_x_);
+
+    NColors::InitColors();
+}
+
+ScoutMonitor::~ScoutMonitor()
+{
+    delwin(body_info_win_);
+    delwin(system_info_win_);
+    endwin();
+}
+
+void ScoutMonitor::UpdateAll()
+{
+    ClearAll();
+
+    CalcDimensions();
+    if (resizing_detected_)
+        HandleResizing();
+
+    UpdateScoutBodyInfo();
+    UpdateScoutSystemInfo();
+}
+
+void ScoutMonitor::SetTestStateData()
+{
+    scout_state_.base_state = BASE_NORMAL;
+    scout_state_.battery_voltage = 28.5;
+
+    scout_state_.linear_velocity = 1.234;
+    scout_state_.angular_velocity = -0.6853;
+
+    // scout_state_.fault_code |= MOTOR_DRV_OVERHEAT_W;
+    // scout_state_.fault_code |= MOTOR_OVERCURRENT_W;
+    // scout_state_.fault_code |= MOTOR_DRV_OVERHEAT_F;
+    // scout_state_.fault_code |= MOTOR_OVERCURRENT_F;
+    // scout_state_.fault_code |= BAT_UNDER_VOL_W;
+    // scout_state_.fault_code |= BAT_UNDER_VOL_F;
+    scout_state_.fault_code = 0x0000;
+    // scout_state_.fault_code = 0xffff;
+
+    // scout_state_.front_light_state.mode = CONST_ON;
+    scout_state_.front_light_state.mode = CUSTOM;
+    scout_state_.front_light_state.custom_value = 50;
+
+    scout_state_.rear_light_state.mode = CONST_ON;
+
+    scout_state_.motor_states[0].current = 10.1;
+    scout_state_.motor_states[0].rpm = 2000;
+    scout_state_.motor_states[0].temperature = 35;
+
+    scout_state_.motor_states[1].current = 10.1;
+    scout_state_.motor_states[1].rpm = 2000;
+    scout_state_.motor_states[1].temperature = 35;
+
+    scout_state_.motor_states[2].current = 10.1;
+    scout_state_.motor_states[2].rpm = 2000;
+    scout_state_.motor_states[2].temperature = 35;
+
+    scout_state_.motor_states[3].current = 10.1;
+    scout_state_.motor_states[3].rpm = 2000;
+    scout_state_.motor_states[3].temperature = 35;
+}
+
+void ScoutMonitor::Run(std::string device_name)
+{
+    if (device_name != "")
+        test_mode_ = false;
+
+    if (test_mode_)
+        SetTestStateData();
+    else
+        scout_base_.ConnectCANBus(device_name);
+
+    StopWatch sw;
+    while (keep_running_)
+    {
+        // label starting point of iteration
+        sw.tic();
+
+        // query for latest robot state
+        if (!test_mode_)
+            scout_state_ = scout_base_.GetScoutState();
+
+        // update window contents
+        UpdateAll();
+
+        // manage window refresh rate
+        sw.sleep_until_ms(100);
+    }
+}
+
+void ScoutMonitor::CalcDimensions()
+{
+    int sy, sx;
+    getmaxyx(stdscr, sy, sx);
+
+    if (sy != term_sy_ || sx != term_sx_)
+    {
+        resizing_detected_ = true;
+
+        term_sy_ = sy;
+        term_sx_ = sx;
+
+        bi_win_sy_ = term_sy_;
+        bi_win_sx_ = term_sx_ * 15 / 24;
+        bi_origin_y_ = 0;
+        bi_origin_x_ = 0;
+
+        si_win_sy_ = term_sy_;
+        si_win_sx_ = term_sx_ * 9 / 24;
+        si_origin_y_ = 0;
+        si_origin_x_ = bi_win_sx_;
+    }
+}
+
+void ScoutMonitor::HandleResizing()
+{
+    delwin(body_info_win_);
+    delwin(system_info_win_);
+
+    body_info_win_ = newwin(bi_win_sy_, bi_win_sx_, bi_origin_y_, bi_origin_x_);
+    system_info_win_ = newwin(si_win_sy_, si_win_sx_, si_origin_y_, si_origin_x_);
+
+    resizing_detected_ = false;
+}
+
+void ScoutMonitor::ClearAll()
+{
+    wclear(body_info_win_);
+    wclear(system_info_win_);
+}
+
+void ScoutMonitor::ShowVehicleState(int y, int x)
+{
+    // show linear velocity
+    const int linear_axis_x = x + vehicle_fp_offset_x_;
+    const int linear_axis_tip_y = y + 2;
+    const int linear_axis_origin_y = linear_axis_tip_y + linear_axis_length_;
+    const int linear_axis_negative_y = linear_axis_origin_y + linear_axis_length_ + 1;
+    mvwprintw(body_info_win_, linear_axis_tip_y - 1, linear_axis_x, "^");
+    for (int i = linear_axis_tip_y; i < linear_axis_origin_y; ++i)
+        mvwprintw(body_info_win_, i, linear_axis_x, "-");
+    mvwprintw(body_info_win_, linear_axis_origin_y, linear_axis_x, "x");
+    for (int i = linear_axis_origin_y + 1; i < linear_axis_negative_y; ++i)
+        mvwprintw(body_info_win_, i, linear_axis_x, "-");
+    mvwprintw(body_info_win_, linear_axis_negative_y, linear_axis_x, "v");
+    double linear_percentage = scout_state_.linear_velocity / ScoutMotionCmd::max_linear_velocity;
+    int linear_bars = std::abs(static_cast<int>(linear_percentage * 5)) + 1;
+    if (std::abs(scout_state_.linear_velocity) < 0.001)
+        linear_bars = 0;
+    if (linear_bars > 5)
+        linear_bars = 5;
+    if (scout_state_.linear_velocity > 0)
+    {
+        for (int i = linear_axis_origin_y - linear_bars; i < linear_axis_origin_y; ++i)
+        {
+            NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::CYAN);
+            mvwprintw(body_info_win_, i, linear_axis_x, "-");
+            NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::CYAN);
+        }
+    }
+    else if (scout_state_.linear_velocity < 0)
+    {
+        for (int i = linear_axis_origin_y + linear_bars; i > linear_axis_origin_y; --i)
+        {
+            NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::CYAN);
+            mvwprintw(body_info_win_, i, linear_axis_x, "-");
+            NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::CYAN);
+        }
+    }
+
+    // show angular velocity
+    const int angular_axis_y = linear_axis_origin_y;
+    const int angular_axis_origin_x = linear_axis_x;
+    const int angular_axis_positive_x = angular_axis_origin_x + angular_axis_length_ + 1;
+    const int angular_axis_negative_x = angular_axis_origin_x - angular_axis_length_;
+    mvwprintw(body_info_win_, angular_axis_y, angular_axis_negative_x - 1, "<");
+    for (int i = angular_axis_negative_x; i < angular_axis_origin_x; ++i)
+        mvwprintw(body_info_win_, angular_axis_y, i, "-");
+    mvwprintw(body_info_win_, linear_axis_origin_y, linear_axis_x, "x");
+    for (int i = angular_axis_origin_x + 1; i < angular_axis_positive_x; ++i)
+        mvwprintw(body_info_win_, angular_axis_y, i, "-");
+    mvwprintw(body_info_win_, angular_axis_y, angular_axis_positive_x, ">");
+
+    double angular_percentage = scout_state_.angular_velocity / ScoutMotionCmd::max_angular_velocity;
+    int angular_bars = std::abs(static_cast<int>(angular_percentage * 5)) + 1;
+    if (std::abs(scout_state_.angular_velocity) < 0.001)
+        angular_bars = 0;
+    if (angular_bars > 5)
+        angular_bars = 5;
+    if (scout_state_.angular_velocity < 0)
+    {
+        for (int i = angular_axis_origin_x + angular_bars; i > angular_axis_origin_x; --i)
+        {
+            NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA);
+            mvwprintw(body_info_win_, angular_axis_y, i, "-");
+            NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA);
+        }
+    }
+    else if (scout_state_.angular_velocity > 0)
+    {
+        for (int i = angular_axis_origin_x - angular_bars; i < angular_axis_origin_x; ++i)
+        {
+            NColors::WSetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA);
+            mvwprintw(body_info_win_, angular_axis_y, i, "-");
+            NColors::WUnsetColor(body_info_win_, NColors::BLACK, NColors::MAGENTA);
+        }
+    }
+
+    // show velocity values
+    std::string linear_vel_str = "linear : " + ConvertFloatToString(scout_state_.linear_velocity);
+    mvwprintw(body_info_win_, linear_axis_negative_y + 2, angular_axis_negative_x - 2, linear_vel_str.c_str());
+
+    std::string angular_vel_str = "angular: " + ConvertFloatToString(scout_state_.angular_velocity);
+    mvwprintw(body_info_win_, linear_axis_negative_y + 3, angular_axis_negative_x - 2, angular_vel_str.c_str());
+
+    // show vehicle base
+    NShapes::WDrawRectangle(body_info_win_, linear_axis_tip_y - 2, angular_axis_negative_x - 4,
+                           linear_axis_negative_y + 4, angular_axis_positive_x + 3);
+
+    // show vehicle wheels
+    NShapes::WDrawRectangle(body_info_win_, linear_axis_tip_y - 1, angular_axis_negative_x - 9,
+                           linear_axis_tip_y + 4, angular_axis_negative_x - 5);
+    NShapes::WDrawRectangle(body_info_win_, linear_axis_negative_y - 2, angular_axis_negative_x - 9,
+                           linear_axis_negative_y + 3, angular_axis_negative_x - 5);
+    NShapes::WDrawRectangle(body_info_win_, linear_axis_tip_y - 1, angular_axis_positive_x + 4,
+                           linear_axis_tip_y + 4, angular_axis_positive_x + 8);
+    NShapes::WDrawRectangle(body_info_win_, linear_axis_negative_y - 2, angular_axis_positive_x + 4,
+                           linear_axis_negative_y + 3, angular_axis_positive_x + 8);
+
+    // front right motor
+    ShowMotorInfo(linear_axis_tip_y - 1, angular_axis_positive_x + 4, scout_state_.motor_states[0].current,
+                  scout_state_.motor_states[0].rpm, scout_state_.motor_states[0].temperature, true);
+    // front left motor
+    ShowMotorInfo(linear_axis_tip_y - 1, angular_axis_negative_x - 9, scout_state_.motor_states[1].current,
+                  scout_state_.motor_states[1].rpm, scout_state_.motor_states[1].temperature, false);
+    // rear left motor
+    ShowMotorInfo(linear_axis_negative_y - 2, angular_axis_negative_x - 9, scout_state_.motor_states[2].current,
+                  scout_state_.motor_states[2].rpm, scout_state_.motor_states[2].temperature, false);
+    // rear right motor
+    ShowMotorInfo(linear_axis_negative_y - 2, angular_axis_positive_x + 4, scout_state_.motor_states[3].current,
+                  scout_state_.motor_states[3].rpm, scout_state_.motor_states[3].temperature, true);
+
+    // show vehicle lights
+    std::string front_mode_str = "Mode: ";
+    if (scout_state_.front_light_state.mode == CONST_ON)
+        front_mode_str += "ON";
+    else if (scout_state_.front_light_state.mode == CONST_OFF)
+        front_mode_str += "OFF";
+    else if (scout_state_.front_light_state.mode == BREATH)
+        front_mode_str += "BREATH";
+    else if (scout_state_.front_light_state.mode == CUSTOM)
+        front_mode_str += "CUSTOM";
+    mvwprintw(body_info_win_, linear_axis_tip_y - 4, angular_axis_origin_x - 13, front_mode_str.c_str());
+    std::string front_custom_str = "Custom: " + ConvertFloatToString(scout_state_.front_light_state.custom_value, 0);
+    mvwprintw(body_info_win_, linear_axis_tip_y - 4, angular_axis_origin_x + 3, front_custom_str.c_str());
+    if (scout_state_.front_light_state.mode != CONST_OFF &&
+        !(scout_state_.front_light_state.mode == CUSTOM && scout_state_.front_light_state.custom_value == 0))
+    {
+        NColors::WSetColor(body_info_win_, NColors::BRIGHT_YELLOW);
+        for (int i = angular_axis_origin_x - 5; i < angular_axis_origin_x - 1; ++i)
+            mvwprintw(body_info_win_, linear_axis_tip_y - 3, i, "v");
+        mvwprintw(body_info_win_, linear_axis_tip_y - 3, angular_axis_origin_x, "v");
+        for (int i = angular_axis_origin_x + 2; i <= angular_axis_origin_x + 5; ++i)
+            mvwprintw(body_info_win_, linear_axis_tip_y - 3, i, "v");
+        NColors::WUnsetColor(body_info_win_, NColors::BRIGHT_YELLOW);
+    }
+
+    std::string rear_mode_str = "Mode: ";
+    if (scout_state_.rear_light_state.mode == CONST_ON)
+        rear_mode_str += "ON";
+    else if (scout_state_.rear_light_state.mode == CONST_OFF)
+        rear_mode_str += "OFF";
+    else if (scout_state_.rear_light_state.mode == BREATH)
+        rear_mode_str += "BREATH";
+    else if (scout_state_.rear_light_state.mode == CUSTOM)
+        rear_mode_str += "CUSTOM";
+    mvwprintw(body_info_win_, linear_axis_negative_y + 6, angular_axis_origin_x - 13, rear_mode_str.c_str());
+    std::string rear_custom_str = "Custom: " + ConvertFloatToString(scout_state_.rear_light_state.custom_value, 0);
+    mvwprintw(body_info_win_, linear_axis_negative_y + 6, angular_axis_origin_x + 3, rear_custom_str.c_str());
+    if (scout_state_.rear_light_state.mode != CONST_OFF &&
+        !(scout_state_.rear_light_state.mode == CUSTOM && scout_state_.rear_light_state.custom_value == 0))
+    {
+        NColors::WSetColor(body_info_win_, NColors::BRIGHT_RED);
+        for (int i = angular_axis_origin_x - 5; i < angular_axis_origin_x - 1; ++i)
+            mvwprintw(body_info_win_, linear_axis_negative_y + 5, i, "^");
+        mvwprintw(body_info_win_, linear_axis_negative_y + 5, angular_axis_origin_x, "^");
+        for (int i = angular_axis_origin_x + 2; i <= angular_axis_origin_x + 5; ++i)
+            mvwprintw(body_info_win_, linear_axis_negative_y + 5, i, "^");
+        NColors::WUnsetColor(body_info_win_, NColors::BRIGHT_RED);
+    }
+}
+
+void ScoutMonitor::UpdateScoutBodyInfo()
+{
+    // for (int i = 0; i < bi_win_sx_; i++)
+    //     mvwprintw(body_info_win_, bi_win_sy_ - 1, i, "-");
+
+    ShowVehicleState(bi_win_sy_ / 2 - vehicle_fp_offset_y_, bi_win_sx_ / 2 - vehicle_fp_offset_x_);
+
+    wrefresh(body_info_win_);
+}
+
+void ScoutMonitor::UpdateScoutSystemInfo()
+{
+    for (int i = 0; i < si_win_sy_; i++)
+        mvwprintw(system_info_win_, i, 0, "|");
+
+    const int state_title_col = (si_win_sx_ - 24) / 2;
+    const int state_value_col = state_title_col + 20;
+    const int state_div_col = state_value_col - 2;
+
+    // system state
+    const int sec1 = static_cast<int>(std::round((si_win_sy_ - 20) / 2.0));
+    ShowStatusItemName(sec1, state_title_col, "System state");
+
+    if (scout_state_.base_state == BASE_NORMAL)
+    {
+        NColors::WSetColor(system_info_win_, NColors::GREEN);
+        mvwprintw(system_info_win_, sec1, state_value_col, "NORMAL");
+        NColors::WUnsetColor(system_info_win_, NColors::GREEN);
+    }
+    else if (scout_state_.base_state == BASE_ESTOP)
+    {
+        NColors::WSetColor(system_info_win_, NColors::YELLOW);
+        mvwprintw(system_info_win_, sec1, state_value_col, "ESTOP");
+        NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
+    }
+    else if (scout_state_.base_state == BASE_EXCEPTION)
+    {
+        NColors::WSetColor(system_info_win_, NColors::RED);
+        mvwprintw(system_info_win_, sec1, state_value_col, "EXCEPT");
+        NColors::WUnsetColor(system_info_win_, NColors::RED);
+    }
+
+    // control mode
+    ShowStatusItemName(sec1 + 1, state_title_col, "Control mode");
+    if (scout_state_.control_mode == REMOTE_MODE)
+        mvwprintw(system_info_win_, sec1 + 1, state_value_col, "REMOTE");
+    else if (scout_state_.control_mode == CMD_CAN_MODE)
+        mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CAN");
+    else if (scout_state_.control_mode == CMD_UART_MODE)
+        mvwprintw(system_info_win_, sec1 + 1, state_value_col, "UART");
+    // mvwprintw(system_info_win_, sec1 + 1, state_value_col, std::to_string(scout_state_.control_mode).c_str());
+
+    // battery voltage
+    ShowStatusItemName(sec1 + 2, state_title_col, "Battery voltage");
+    std::string bat_vol_str = ConvertFloatToString(scout_state_.battery_voltage, 1) + " v";
+    mvwprintw(system_info_win_, sec1 + 2, state_value_col, bat_vol_str.c_str());
+
+    const int fault_col_1 = state_value_col;
+    const int fault_col_2 = fault_col_1 + 2;
+    const int fault_col_3 = fault_col_2 + 2;
+
+    const int sec2 = sec1 + 4;
+    mvwprintw(system_info_win_, sec2, state_title_col, "System faults");
+
+    // motor driver over heat;
+    ShowStatusItemName(sec2 + 1, state_title_col, "-Drv over-heat");
+    if ((scout_state_.fault_code & MOTOR_DRV_OVERHEAT_W) == 0 &&
+        (scout_state_.fault_code & MOTOR_DRV_OVERHEAT_F) == 0)
+    {
+        NColors::WSetColor(system_info_win_, NColors::GREEN);
+        mvwprintw(system_info_win_, sec2 + 1, fault_col_1, "N");
+        NColors::WUnsetColor(system_info_win_, NColors::GREEN);
+    }
+    else
+    {
+        if (scout_state_.fault_code & MOTOR_DRV_OVERHEAT_W)
+        {
+            NColors::WSetColor(system_info_win_, NColors::YELLOW);
+            mvwprintw(system_info_win_, sec2 + 1, fault_col_2, "W");
+            NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
+        }
+        if (scout_state_.fault_code & MOTOR_DRV_OVERHEAT_F)
+        {
+            NColors::WSetColor(system_info_win_, NColors::RED);
+            mvwprintw(system_info_win_, sec2 + 1, fault_col_3, "P");
+            NColors::WUnsetColor(system_info_win_, NColors::RED);
+        }
+    }
+
+    // motor driver over current
+    ShowStatusItemName(sec2 + 2, state_title_col, "-Mt over-current");
+    if ((scout_state_.fault_code & MOTOR_OVERCURRENT_W) == 0 &&
+        (scout_state_.fault_code & MOTOR_OVERCURRENT_F) == 0)
+    {
+        NColors::WSetColor(system_info_win_, NColors::GREEN);
+        mvwprintw(system_info_win_, sec2 + 2, fault_col_1, "N");
+        NColors::WUnsetColor(system_info_win_, NColors::GREEN);
+    }
+    else
+    {
+        if (scout_state_.fault_code & MOTOR_OVERCURRENT_W)
+        {
+            NColors::WSetColor(system_info_win_, NColors::YELLOW);
+            mvwprintw(system_info_win_, sec2 + 2, fault_col_2, "W");
+            NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
+        }
+        if (scout_state_.fault_code & MOTOR_OVERCURRENT_F)
+        {
+            NColors::WSetColor(system_info_win_, NColors::RED);
+            mvwprintw(system_info_win_, sec2 + 2, fault_col_3, "P");
+            NColors::WUnsetColor(system_info_win_, NColors::RED);
+        }
+    }
+
+    // battery under voltage
+    ShowStatusItemName(sec2 + 3, state_title_col, "-Bat under volt");
+    if ((scout_state_.fault_code & BAT_UNDER_VOL_W) == 0 &&
+        (scout_state_.fault_code & BAT_UNDER_VOL_F) == 0)
+    {
+        NColors::WSetColor(system_info_win_, NColors::GREEN);
+        mvwprintw(system_info_win_, sec2 + 3, fault_col_1, "N");
+        NColors::WUnsetColor(system_info_win_, NColors::GREEN);
+    }
+    else
+    {
+        if (scout_state_.fault_code & BAT_UNDER_VOL_W)
+        {
+            NColors::WSetColor(system_info_win_, NColors::YELLOW);
+            mvwprintw(system_info_win_, sec2 + 3, fault_col_2, "W");
+            NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
+        }
+        if (scout_state_.fault_code & BAT_UNDER_VOL_F)
+        {
+            NColors::WSetColor(system_info_win_, NColors::RED);
+            mvwprintw(system_info_win_, sec2 + 3, fault_col_3, "F");
+            NColors::WUnsetColor(system_info_win_, NColors::RED);
+        }
+    }
+
+    // battery over voltage
+    ShowStatusItemName(sec2 + 4, state_title_col, "-Bat over volt");
+    ShowFault(sec2 + 4, fault_col_1, (scout_state_.fault_code & BAT_OVER_VOL_F) == 0);
+
+    const int sec3 = sec2 + 6;
+    mvwprintw(system_info_win_, sec3, state_title_col, "Comm faults");
+
+    // CAN cmd checksum
+    ShowStatusItemName(sec3 + 1, state_title_col, "-CAN cmd checksum");
+    ShowFault(sec3 + 1, fault_col_1, (scout_state_.fault_code & CAN_CHECKSUM_ERROR) == 0);
+
+    // motor comm
+    ShowStatusItemName(sec3 + 2, state_title_col, "-Motor 1 comm");
+    ShowFault(sec3 + 2, fault_col_1, (scout_state_.fault_code & MOTOR1_COMM_F) == 0);
+
+    ShowStatusItemName(sec3 + 3, state_title_col, "-Motor 2 comm");
+    ShowFault(sec3 + 3, fault_col_1, (scout_state_.fault_code & MOTOR2_COMM_F) == 0);
+
+    ShowStatusItemName(sec3 + 4, state_title_col, "-Motor 3 comm");
+    ShowFault(sec3 + 4, fault_col_1, (scout_state_.fault_code & MOTOR3_COMM_F) == 0);
+
+    ShowStatusItemName(sec3 + 5, state_title_col, "-Motor 4 comm");
+    ShowFault(sec3 + 5, fault_col_1, (scout_state_.fault_code & MOTOR4_COMM_F) == 0);
+
+    const int sec4 = sec3 + 8;
+    NColors::WSetColor(system_info_win_, NColors::GREEN);
+    mvwprintw(system_info_win_, sec4, state_title_col + 1, "N: normal");
+    NColors::WUnsetColor(system_info_win_, NColors::GREEN);
+
+    NColors::WSetColor(system_info_win_, NColors::YELLOW);
+    mvwprintw(system_info_win_, sec4, state_title_col + 12, "W: warning");
+    NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
+
+    NColors::WSetColor(system_info_win_, NColors::RED);
+    mvwprintw(system_info_win_, sec4 + 1, state_title_col + 1, "F: fault   P: protection");
+    NColors::WUnsetColor(system_info_win_, NColors::RED);
+
+    wrefresh(system_info_win_);
+}
+
+void ScoutMonitor::ShowStatusItemName(int y, int x, std::string name)
+{
+    const int state_value_col = x + 20;
+    const int state_div_col = state_value_col - 2;
+
+    mvwprintw(system_info_win_, y, x, name.c_str());
+    mvwprintw(system_info_win_, y, state_div_col, ":");
+}
+
+void ScoutMonitor::ShowFault(int y, int x, bool no_fault)
+{
+    const int fault_col_1 = x;
+    const int fault_col_2 = x + 2;
+    const int fault_col_3 = fault_col_2 + 2;
+
+    if (no_fault)
+    {
+        NColors::WSetColor(system_info_win_, NColors::GREEN);
+        mvwprintw(system_info_win_, y, fault_col_1, "N");
+        NColors::WUnsetColor(system_info_win_, NColors::GREEN);
+    }
+    else
+    {
+        NColors::WSetColor(system_info_win_, NColors::RED);
+        mvwprintw(system_info_win_, y, fault_col_3, "F");
+        NColors::WUnsetColor(system_info_win_, NColors::RED);
+    }
+}
+
+// (y,x): position of the top left point of corresponding wheel
+void ScoutMonitor::ShowMotorInfo(int y, int x, double cur, int rpm, int temp, bool is_right)
+{
+    int col_title = x;
+    if (is_right)
+        col_title += 6;
+    else
+        col_title -= 9;
+
+    std::string cur_str = "CUR:" + ConvertFloatToString(cur, 1);
+    mvwprintw(body_info_win_, y + 1, col_title, cur_str.c_str());
+
+    std::string rpm_str = "RPM:" + ConvertFloatToString(rpm, 0);
+    mvwprintw(body_info_win_, y + 2, col_title, rpm_str.c_str());
+
+    std::string temp_str = "TMP:" + ConvertFloatToString(temp, 0);
+    mvwprintw(body_info_win_, y + 3, col_title, temp_str.c_str());
+}
+} // namespace wescore

scout_base/src/scout_sdk/src/monitor/tests/CMakeLists.txt

@@ -0,0 +1,15 @@
+# Add executables
+add_executable(test_scout_monitor test_scout_monitor.cpp)
+target_link_libraries(test_scout_monitor monitor)
+
+add_executable(test_scout_monitor_virtual test_scout_monitor_virtual.cpp)
+target_link_libraries(test_scout_monitor_virtual monitor)
+
+# add_executable(test_ncurses test_ncurses.cpp)
+# target_link_libraries(test_ncurses monitor)
+
+add_executable(test_ncolor test_ncolor.c)
+target_link_libraries(test_ncolor monitor)
+
+add_executable(test_ncolor2 test_ncolor2.cpp)
+target_link_libraries(test_ncolor2 monitor)

scout_base/src/scout_sdk/src/monitor/tests/test_ncolor.c

@@ -0,0 +1,158 @@
+/* color-demo.c */
+// source: https://www.linuxjournal.com/content/about-ncurses-colors-0
+
+#include <curses.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+int is_bold(int fg);
+void init_colorpairs(void);
+short curs_color(int fg);
+int colornum(int fg, int bg);
+void setcolor(int fg, int bg);
+void unsetcolor(int fg, int bg);
+
+void init_colorpairs(void)
+{
+    int fg, bg;
+    int colorpair;
+
+    for (bg = 0; bg <= 7; bg++)
+    {
+        for (fg = 0; fg <= 7; fg++)
+        {
+            colorpair = colornum(fg, bg);
+            init_pair(colorpair, curs_color(fg), curs_color(bg));
+        }
+    }
+}
+
+short curs_color(int fg)
+{
+    switch (7 & fg)
+    {       /* RGB */
+    case 0: /* 000 */
+        return (COLOR_BLACK);
+    case 1: /* 001 */
+        return (COLOR_BLUE);
+    case 2: /* 010 */
+        return (COLOR_GREEN);
+    case 3: /* 011 */
+        return (COLOR_CYAN);
+    case 4: /* 100 */
+        return (COLOR_RED);
+    case 5: /* 101 */
+        return (COLOR_MAGENTA);
+    case 6: /* 110 */
+        return (COLOR_YELLOW);
+    case 7: /* 111 */
+        return (COLOR_WHITE);
+    }
+}
+
+int colornum(int fg, int bg)
+{
+    int B, bbb, ffff;
+
+    B = 1 << 7;
+    bbb = (7 & bg) << 4;
+    ffff = 7 & fg;
+
+    return (B | bbb | ffff);
+}
+
+void setcolor(int fg, int bg)
+{
+    /* set the color pair (colornum) and bold/bright (A_BOLD) */
+
+    attron(COLOR_PAIR(colornum(fg, bg)));
+    if (is_bold(fg))
+    {
+        attron(A_BOLD);
+    }
+}
+
+void unsetcolor(int fg, int bg)
+{
+    /* unset the color pair (colornum) and
+       bold/bright (A_BOLD) */
+
+    attroff(COLOR_PAIR(colornum(fg, bg)));
+    if (is_bold(fg))
+    {
+        attroff(A_BOLD);
+    }
+}
+
+int is_bold(int fg)
+{
+    /* return the intensity bit */
+
+    int i;
+
+    i = 1 << 3;
+    return (i & fg);
+}
+
+int main(void)
+{
+    int fg, bg;
+
+    /* initialize curses */
+
+    initscr();
+    keypad(stdscr, TRUE);
+    cbreak();
+    noecho();
+
+    /* initialize colors */
+
+    if (has_colors() == FALSE)
+    {
+        endwin();
+        puts("Your terminal does not support color");
+        exit(1);
+    }
+
+    start_color();
+    init_colorpairs();
+
+    /* draw test pattern */
+
+    if ((LINES < 24) || (COLS < 80))
+    {
+        endwin();
+        puts("Your terminal needs to be at least 80x24");
+        exit(2);
+    }
+
+    mvaddstr(0, 35, "COLOR DEMO");
+    mvaddstr(2, 0, "low intensity text colors (0-7)");
+    mvaddstr(12, 0, "high intensity text colors (8-15)");
+
+    for (bg = 0; bg <= 7; bg++)
+    {
+        for (fg = 0; fg <= 7; fg++)
+        {
+            setcolor(fg, bg);
+            mvaddstr(fg + 3, bg * 10, "...test...");
+            unsetcolor(fg, bg);
+        }
+
+        for (fg = 8; fg <= 15; fg++)
+        {
+            setcolor(fg, bg);
+            mvaddstr(fg + 5, bg * 10, "...test...");
+            unsetcolor(fg, bg);
+        }
+    }
+
+    mvaddstr(LINES - 1, 0, "press any key to quit");
+
+    refresh();
+
+    getch();
+    endwin();
+
+    exit(0);
+}

scout_base/src/scout_sdk/src/monitor/tests/test_ncolor2.cpp

@@ -0,0 +1,65 @@
+#include "monitor/ncolors.hpp"
+
+using namespace wescore;
+
+int main(void)
+{
+    int fg, bg;
+
+    /* initialize curses */
+
+    initscr();
+    keypad(stdscr, TRUE);
+    cbreak();
+    noecho();
+
+    /* initialize colors */
+
+    if (has_colors() == FALSE)
+    {
+        endwin();
+        puts("Your terminal does not support color");
+        return 1;
+    }
+
+    NColors::InitColors();
+
+    /* draw test pattern */
+
+    if ((LINES < 24) || (COLS < 80))
+    {
+        endwin();
+        puts("Your terminal needs to be at least 80x24");
+        return 2;
+    }
+
+    mvaddstr(0, 35, "COLOR DEMO");
+    mvaddstr(2, 0, "low intensity text colors (0-7)");
+    mvaddstr(12, 0, "high intensity text colors (8-15)");
+
+    for (bg = 0; bg <= 7; bg++)
+    {
+        for (fg = 0; fg <= 7; fg++)
+        {
+            NColors::SetColor(fg, bg);
+            mvaddstr(fg + 3, bg * 10, "...test...");
+            NColors::UnsetColor(fg, bg);
+        }
+
+        for (fg = 8; fg <= 15; fg++)
+        {
+            NColors::SetColor(fg, bg);
+            mvaddstr(fg + 5, bg * 10, "...test...");
+            NColors::UnsetColor(fg, bg);
+        }
+    }
+
+    mvaddstr(LINES - 1, 0, "press any key to quit");
+
+    refresh();
+
+    getch();
+    endwin();
+
+    return 0;
+}

scout_base/src/scout_sdk/src/monitor/tests/test_ncurses.cpp

@@ -0,0 +1,118 @@
+#include <panel.h>
+#include <string.h>
+
+#define NLINES 10
+#define NCOLS 40
+
+void init_wins(WINDOW **wins, int n);
+void win_show(WINDOW *win, char *label, int label_color);
+void print_in_middle(WINDOW *win, int starty, int startx, int width, char *string, chtype color);
+
+int main()
+{	WINDOW *my_wins[3];
+	PANEL  *my_panels[3];
+	PANEL  *top;
+	int ch;
+
+	/* Initialize curses */
+	initscr();
+	start_color();
+	cbreak();
+	noecho();
+	keypad(stdscr, TRUE);
+
+	/* Initialize all the colors */
+	init_pair(1, COLOR_RED, COLOR_BLACK);
+	init_pair(2, COLOR_GREEN, COLOR_BLACK);
+	init_pair(3, COLOR_BLUE, COLOR_BLACK);
+	init_pair(4, COLOR_CYAN, COLOR_BLACK);
+
+	init_wins(my_wins, 3);
+	
+	/* Attach a panel to each window */ 	/* Order is bottom up */
+	my_panels[0] = new_panel(my_wins[0]); 	/* Push 0, order: stdscr-0 */
+	my_panels[1] = new_panel(my_wins[1]); 	/* Push 1, order: stdscr-0-1 */
+	my_panels[2] = new_panel(my_wins[2]); 	/* Push 2, order: stdscr-0-1-2 */
+
+	/* Set up the user pointers to the next panel */
+	set_panel_userptr(my_panels[0], my_panels[1]);
+	set_panel_userptr(my_panels[1], my_panels[2]);
+	set_panel_userptr(my_panels[2], my_panels[0]);
+
+	/* Update the stacking order. 2nd panel will be on top */
+	update_panels();
+
+	/* Show it on the screen */
+	attron(COLOR_PAIR(4));
+	mvprintw(LINES - 2, 0, "Use tab to browse through the windows (F1 to Exit)");
+	attroff(COLOR_PAIR(4));
+	doupdate();
+
+	top = my_panels[2];
+	while((ch = getch()) != KEY_F(1))
+	{	switch(ch)
+		{	case 9:
+				top = (PANEL *)panel_userptr(top);
+				top_panel(top);
+				break;
+		}
+		update_panels();
+		doupdate();
+	}
+	endwin();
+	return 0;
+}
+
+/* Put all the windows */
+void init_wins(WINDOW **wins, int n)
+{	int x, y, i;
+	char label[80];
+
+	y = 2;
+	x = 10;
+	for(i = 0; i < n; ++i)
+	{	wins[i] = newwin(NLINES, NCOLS, y, x);
+		sprintf(label, "Window Number %d", i + 1);
+		win_show(wins[i], label, i + 1);
+		y += 3;
+		x += 7;
+	}
+}
+
+/* Show the window with a border and a label */
+void win_show(WINDOW *win, char *label, int label_color)
+{	int startx, starty, height, width;
+
+	getbegyx(win, starty, startx);
+	getmaxyx(win, height, width);
+
+	box(win, 0, 0);
+	mvwaddch(win, 2, 0, ACS_LTEE); 
+	mvwhline(win, 2, 1, ACS_HLINE, width - 2); 
+	mvwaddch(win, 2, width - 1, ACS_RTEE); 
+	
+	print_in_middle(win, 1, 0, width, label, COLOR_PAIR(label_color));
+}
+
+void print_in_middle(WINDOW *win, int starty, int startx, int width, char *string, chtype color)
+{	int length, x, y;
+	float temp;
+
+	if(win == NULL)
+		win = stdscr;
+	getyx(win, y, x);
+	if(startx != 0)
+		x = startx;
+	if(starty != 0)
+		y = starty;
+	if(width == 0)
+		width = 80;
+
+	length = strlen(string);
+	temp = (width - length)/ 2;
+	x = startx + (int)temp;
+	wattron(win, color);
+	mvwprintw(win, y, x, "%s", string);
+	wattroff(win, color);
+	refresh();
+}

scout_base/src/scout_sdk/src/monitor/tests/test_scout_monitor.cpp

@@ -0,0 +1,33 @@
+#include <signal.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <iostream>
+
+#include "monitor/scout_monitor.hpp"
+
+using namespace wescore;
+
+ScoutMonitor monitor;
+
+void SignalHandler(int s)
+{
+    printf("Caught signal %d\n", s);
+    monitor.Terminate();
+    exit(1);
+}
+
+int main(int argc, char **argv)
+{
+    struct sigaction sigIntHandler;
+    sigIntHandler.sa_handler = SignalHandler;
+    sigemptyset(&sigIntHandler.sa_mask);
+    sigIntHandler.sa_flags = 0;
+    sigaction(SIGINT, &sigIntHandler, NULL);
+
+    std::cout << "scout monitor started" << std::endl;
+    monitor.Run("can1");
+
+    return 0;
+}

scout_base/src/scout_sdk/src/monitor/tests/test_scout_monitor_virtual.cpp

@@ -0,0 +1,33 @@
+#include <signal.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <iostream>
+
+#include "monitor/scout_monitor.hpp"
+
+using namespace wescore;
+
+ScoutMonitor monitor;
+
+void SignalHandler(int s)
+{
+    printf("Caught signal %d\n", s);
+    monitor.Terminate();
+    exit(1);
+}
+
+int main(int argc, char **argv)
+{
+    struct sigaction sigIntHandler;
+    sigIntHandler.sa_handler = SignalHandler;
+    sigemptyset(&sigIntHandler.sa_mask);
+    sigIntHandler.sa_flags = 0;
+    sigaction(SIGINT, &sigIntHandler, NULL);
+
+    std::cout << "scout monitor started" << std::endl;
+    monitor.Run();
+
+    return 0;
+}

scout_base/src/scout_sdk/src/scout/CMakeLists.txt

@@ -1,40 +0,0 @@
-cmake_minimum_required(VERSION 3.0.0)
-project(scout_base)
-
-## Put all binary files into /bin and libraries into /lib
-set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
-set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
-set(ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
-
-## Set compiler to use c++ 11 features
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX14)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
-elseif(COMPILER_SUPPORTS_CXX11)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-else()
-	message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 or C++11 support. Please use a different C++ compiler.")
-endif()
-
-## Add libraries
-set(SCOUT_BASE_SRC 
-    src/scout_base.cpp
-    # src/scout_serial.cpp
-)
-add_library(scout_base STATIC ${SCOUT_BASE_SRC})
-target_link_libraries(scout_base asyncio stopwatch)
-target_include_directories(scout_base PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<BUILD_INTERFACE:${Boost_INCLUDE_DIRS}>
-	$<INSTALL_INTERFACE:include>	
-    PRIVATE src)
-
- ## Add executables
-if(BUILD_TESTS)
-    add_subdirectory(tests)
-endif()

scout_base/src/scout_sdk/src/scout/tests/CMakeLists.txt

@@ -1,12 +0,0 @@
-# Dependency libraries
-#find_package(LIBRARY_NAME REQUIRED)
-
-# tests
-# add_executable(test_scout_cpp test_scout_cpp.cpp)
-# target_link_libraries(test_scout_cpp scout stopwatch)
-
-add_executable(test_scout_base test_scout_base.cpp)
-target_link_libraries(test_scout_base scout_base)
-
-add_executable(test_can_msg test_can_msg.cpp)
-target_link_libraries(test_can_msg scout_base)

scout_base/src/scout_sdk/src/scout/tests/test_scout_cpp.cpp

@@ -1,152 +0,0 @@
-#include <unistd.h>
-
-#include <thread>
-#include <mutex>
-#include <functional>
-#include <string>
-#include <iostream>
-#include <chrono>
-#include <cmath>
-
-#include "stopwatch/stopwatch.h"
-
-#include "scout/scout_base.hpp"
-
-#define TEST_WITHOUT_SERIAL_HARDWARE
-
-using namespace scout;
-
-class ScoutMessenger
-{
-public:
-    ScoutMessenger(ScoutRobot &robot, int32_t ctrl_freq = 10, int32_t odom_query_freq = 200)
-        : robot_(robot), ctrl_freq_(ctrl_freq), odom_query_freq_(odom_query_freq){};
-    ~ScoutMessenger() = default;
-
-    using Clock = std::chrono::high_resolution_clock;
-    using time_point = typename Clock::time_point;
-
-    void PublishOdometry()
-    {
-        ScoutState data;
-        stopwatch::StopWatch swatch;
-        while (true)
-        {
-            swatch.tic();
-
-            // std::cout << "odometry loop" << std::endl;
-
-            if (robot_.QueryRobotState(&data))
-            {
-                short linear = data.linear;
-                short angular = data.angular;
-
-                current_time_ = Clock::now();
-                double dt = std::chrono::duration_cast<std::chrono::seconds>(current_time_ - last_time_).count();
-                static int start_flag = 0;
-
-                //初始化
-                if (start_flag)
-                {
-                    last_time_ = current_time_;
-                    start_flag = 1;
-                    return;
-                }
-
-                double Angular = angular;
-                double Linear = linear;
-                Angular /= 10000;
-                Linear /= 10000;
-
-                vth = Angular; //旋转角速度，在角度很小的情况下。约等于（SPEED右 - SPEED左）/ 轮子间距
-                vx = Linear;   //直线X轴的速度 =  （SPEED右 - SPEED左） / 2
-                vy = 0.0;      //2轮差速车，不能左右横移
-
-                double delta_x = (vx * std::cos(th) - vy * std::sin(th)) * dt;
-                double delta_y = (vx * std::sin(th) + vy * std::cos(th)) * dt;
-                double delta_th = vth * dt;
-
-                x += delta_x;
-                y += delta_y;
-                th += delta_th;
-
-                std::cout << "pose: (x,y) " << x << " , " << y << " ; yaw: " << th << std::endl;
-                std::cout << "velocity: linear " << vx << " , angular " << vth << std::endl;
-
-                last_time_ = current_time_;
-            }
-
-            swatch.sleep_until_ms(1000.0 / odom_query_freq_);
-        }
-    }
-
-    void UpdateCommandCallback()
-    {
-        // Update linear_x, angular_z via your IPC method
-        // Note: be careful about the sync (using mutex)
-        std::lock_guard<std::mutex> lock(cmd_mutex_);
-        cmd_linear_x_ = 0;
-        cmd_angular_z_ = 0;
-    }
-
-    void SendMotionCommand()
-    {
-        stopwatch::StopWatch swatch;
-        static unsigned char index = 0;
-        while (true)
-        {
-            swatch.tic();
-
-            // std::cout << "cmd loop" << std::endl;
-            cmd_mutex_.lock();
-#ifndef TEST_WITHOUT_SERIAL_HARDWARE
-            robot_.SendCommand(cmd_linear_x_, cmd_angular_z_, index++);
-#endif
-            cmd_mutex_.unlock();
-            swatch.sleep_until_ms(1000.0 / ctrl_freq_);
-        }
-    }
-
-private:
-    ScoutRobot &robot_;
-    int32_t ctrl_freq_;
-    int32_t odom_query_freq_;
-
-    std::mutex cmd_mutex_;
-    double cmd_linear_x_ = 0;
-    double cmd_angular_z_ = 0;
-
-    double x = 0;
-    double y = 0;
-    double th = 0;
-    double vx = 0;
-    double vy = 0;
-    double vth = 0;
-
-    time_point current_time_;
-    time_point last_time_;
-};
-
-int main(int argc, char **argv)
-{
-    ScoutRobot scout;
-
-#ifndef TEST_WITHOUT_SERIAL_HARDWARE
-    scout.ConnectSerialPort("/dev/ttyUSB0", 115200);
-
-    if (!scout.IsConnectionActive())
-    {
-        std::cerr << "Failed to connect to robot" << std::endl;
-        return -1;
-    }
-#endif
-
-    ScoutMessenger messenger(scout);
-    std::thread odom_thread(std::bind(&ScoutMessenger::PublishOdometry, &messenger));
-    std::thread cmd_thread(std::bind(&ScoutMessenger::SendMotionCommand, &messenger));
-
-    odom_thread.join();
-    cmd_thread.join();
-
-    return 0;
-}

scout_base/src/scout_sdk/src/sdk_core/CMakeLists.txt

@@ -0,0 +1,7 @@
+# Add source directories
+add_subdirectory(async_io)
+add_subdirectory(scout_base)
+
+if(BUILD_TESTS)
+    add_subdirectory(unit_tests)
+endif()

scout_base/src/scout_sdk/src/sdk_core/async_io/CMakeLists.txt

@@ -0,0 +1,36 @@
+# Dependency libraries
+# find_package(LIB_NAME REQUIRED)
+
+# Add libraries
+set(ASYNC_IO_LIB_SRC
+    src/async_serial.cpp
+    src/async_can.cpp
+)
+add_library(asyncio STATIC ${ASYNC_IO_LIB_SRC})
+set_target_properties(asyncio PROPERTIES POSITION_INDEPENDENT_CODE ON)
+target_compile_definitions(asyncio PUBLIC "-DASIO_ENABLE_OLD_SERVICES -DASIO_HAS_POSIX_STREAM_DESCRIPTOR")
+target_link_libraries(asyncio asio pthread)
+target_include_directories(asyncio PUBLIC
+	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+	$<INSTALL_INTERFACE:include>
+	PRIVATE src)
+
+# Add executables
+if(BUILD_TESTS)
+	add_subdirectory(tests)
+endif()
+
+# # 'make install' to the correct locations (provided by GNUInstallDirs).
+# install(TARGETS lib EXPORT MyLibraryConfig
+# ARCHIVE  DESTINATION ${CMAKE_INSTALL_LIBDIR}
+# LIBRARY  DESTINATION ${CMAKE_INSTALL_LIBDIR}
+# RUNTIME  DESTINATION ${CMAKE_INSTALL_BINDIR})  # This is for Windows
+# install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+
+# # This makes the project importable from the install directory
+# # Put config file in per-project dir (name MUST match), can also
+# # just go into 'cmake'.
+# install(EXPORT MyLibraryConfig DESTINATION share/MyLibrary/cmake)
+
+# # This makes the project importable from the build directory
+# export(TARGETS lib FILE MyLibraryConfig.cmake)

scout_base/src/scout_sdk/src/sdk_core/scout_base/CMakeLists.txt

@@ -0,0 +1,18 @@
+## Add libraries
+set(SCOUT_BASE_SRC 
+    src/scout_base.cpp
+    # src/scout_serial.cpp
+)
+add_library(scoutbase STATIC ${SCOUT_BASE_SRC})
+set_target_properties(scoutbase PROPERTIES POSITION_INDEPENDENT_CODE ON)
+target_link_libraries(scoutbase asyncio)
+target_include_directories(scoutbase PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${Boost_INCLUDE_DIRS}>
+	$<INSTALL_INTERFACE:include>	
+    PRIVATE src)
+
+ ## Add executables
+if(BUILD_TESTS)
+    add_subdirectory(tests)
+endif()

scout_base/src/scout_sdk/src/sdk_core/scout_base/include/scout/scout_base.hpp

@@ -16,7 +16,6 @@
 #include <mutex>
 #include <functional>
 
-#include "stopwatch/stopwatch.h"
 #include "async_io/async_can.hpp"
 
 #include "scout/scout_state.hpp"
@@ -35,10 +34,7 @@ public:
     ScoutBase &operator=(const ScoutBase &scout) = delete;
 
 public:
-    void ConnectSerialPort(const std::string &port_name = "/dev/ttyUSB0", int32_t baud_rate = 115200);
     void ConnectCANBus(const std::string &can_if_name = "can1");
-    bool IsConnectionActive() const { return serial_connected_; }
-
     void StartCmdThread(int32_t period_ms);
 
     // motion control
@@ -52,12 +48,10 @@ public:
     // get robot state
     ScoutState GetScoutState();
 
-    // TODO for testing, will be set private in release
+    // TODO internal use only, for testing, will be set private in future release
     void UpdateScoutState(ScoutState &state, can_frame *rx_frame);
 
 private:
-    bool serial_connected_ = false;
-
     std::shared_ptr<ASyncCAN> can_if_;
 
     std::thread cmd_thread_;

scout_base/src/scout_sdk/src/sdk_core/scout_base/include/scout/scout_can_protocol.h

@@ -44,7 +44,8 @@ static uint8_t Agilex_CANMsgChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
 enum ControlMode
 {
     REMOTE_MODE = 0x00,
-    CMD_MODE = 0x01
+    CMD_CAN_MODE = 0x01,
+    CMD_UART_MODE = 0x02
 };
 
 enum FaultClearFlag
@@ -125,21 +126,14 @@ typedef struct
     union {
         struct StatusDef
         {
-            union {
+            struct {
-                struct
+                uint8_t high_byte;
-                {
+                uint8_t low_byte;
-                    uint8_t high_byte;
-                    uint8_t low_byte;
-                };
-                int16_t value;
             } linear_velocity;
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                int16_t value;
             } angular_velocity;
             uint8_t reserved0;
             uint8_t reserved1;
@@ -181,6 +175,23 @@ enum BaseState
     BASE_EXCEPTION
 };
 
+enum FaultBits
+{
+    CAN_CHECKSUM_ERROR = 0x0100,
+    MOTOR_DRV_OVERHEAT_W = 0x0200,
+    MOTOR_OVERCURRENT_W = 0x0400,
+    BAT_UNDER_VOL_W = 0x0800,
+
+    BAT_UNDER_VOL_F = 0x01,
+    BAT_OVER_VOL_F = 0x02,
+    MOTOR1_COMM_F = 0x04,
+    MOTOR2_COMM_F = 0x08,
+    MOTOR3_COMM_F = 0x10,
+    MOTOR4_COMM_F = 0x20,
+    MOTOR_DRV_OVERHEAT_F = 0x40,
+    MOTOR_OVERCURRENT_F = 0x80
+};
+
 typedef struct
 {
     const uint16_t id = MSG_SYSTEM_STATUS_FEEDBACK_ID;
@@ -190,21 +201,15 @@ typedef struct
         {
             uint8_t base_state;
             uint8_t control_mode;
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                uint16_t value;
             } battery_voltage;
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                uint16_t value;
             } fault_code;
             uint8_t count;
             uint8_t checksum;
@@ -222,21 +227,15 @@ typedef struct
     union {
         struct StatusDef
         {
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                uint16_t value;
             } current;
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                int16_t value;
             } rpm;
             int8_t temperature;
             uint8_t reserved0;
@@ -256,21 +255,15 @@ typedef struct
     union {
         struct StatusDef
         {
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                uint16_t value;
             } current;
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                int16_t value;
             } rpm;
             int8_t temperature;
             uint8_t reserved0;
@@ -290,21 +283,15 @@ typedef struct
     union {
         struct StatusDef
         {
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                uint16_t value;
             } current;
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                int16_t value;
             } rpm;
             int8_t temperature;
             uint8_t reserved0;
@@ -324,21 +311,15 @@ typedef struct
     union {
         struct StatusDef
         {
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                uint16_t value;
             } current;
-            union {
+            struct
-                struct
+            {
-                {
+                uint8_t high_byte;
-                    uint8_t high_byte;
+                uint8_t low_byte;
-                    uint8_t low_byte;
-                };
-                int16_t value;
             } rpm;
             int8_t temperature;
             uint8_t reserved0;

scout_base/src/scout_sdk/src/sdk_core/scout_base/src/scout_base.cpp

@@ -3,9 +3,78 @@
 #include <string>
 #include <cstring>
 #include <iostream>
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ratio>
+#include <thread>
 
 #include "scout/scout_can_protocol.h"
 
+namespace
+{
+// source: https://github.com/rxdu/stopwatch
+struct StopWatch
+{
+    using Clock = std::chrono::high_resolution_clock;
+    using time_point = typename Clock::time_point;
+    using duration = typename Clock::duration;
+
+    StopWatch() { tic_point = Clock::now(); };
+
+    time_point tic_point;
+
+    void tic()
+    {
+        tic_point = Clock::now();
+    };
+
+    double toc()
+    {
+        return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count() / 1000000.0;
+    };
+
+    // for different precisions
+    double stoc()
+    {
+        return std::chrono::duration_cast<std::chrono::seconds>(Clock::now() - tic_point).count();
+    };
+
+    double mtoc()
+    {
+        return std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
+    };
+
+    double utoc()
+    {
+        return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
+    };
+
+    double ntoc()
+    {
+        return std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() - tic_point).count();
+    };
+
+    // you have to call tic() before calling this function
+    void sleep_until_ms(int64_t period_ms)
+    {
+        int64_t duration = period_ms - std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
+
+        if (duration > 0)
+            std::this_thread::sleep_for(std::chrono::milliseconds(duration));
+    };
+
+    void sleep_until_us(int64_t period_us)
+    {
+        int64_t duration = period_us - std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
+
+        if (duration > 0)
+            std::this_thread::sleep_for(std::chrono::microseconds(duration));
+    };
+};
+} // namespace
+
 namespace wescore
 {
 ScoutBase::~ScoutBase()
@@ -14,11 +83,6 @@ ScoutBase::~ScoutBase()
         cmd_thread_.join();
 }
 
-void ScoutBase::ConnectSerialPort(const std::string &port_name, int32_t baud_rate)
-{
-    // serial_connected_ = (scout_serial::Open_Serial(port_name, baud_rate) > 0) ? true : false;
-}
-
 void ScoutBase::ConnectCANBus(const std::string &can_if_name)
 {
     can_if_ = std::make_shared<ASyncCAN>(can_if_name);
@@ -33,7 +97,7 @@ void ScoutBase::StartCmdThread(int32_t period_ms)
 
 void ScoutBase::ControlLoop(int32_t period_ms)
 {
-    stopwatch::StopWatch ctrl_sw;
+    StopWatch ctrl_sw;
     uint8_t cmd_count = 0;
     uint8_t light_cmd_count = 0;
     while (true)
@@ -44,7 +108,7 @@ void ScoutBase::ControlLoop(int32_t period_ms)
         {
             MotionControlMessage m_msg;
 
-            m_msg.data.cmd.control_mode = CMD_MODE;
+            m_msg.data.cmd.control_mode = CMD_CAN_MODE;
 
             motion_cmd_mutex_.lock();
             m_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
@@ -71,14 +135,14 @@ void ScoutBase::ControlLoop(int32_t period_ms)
             LightControlMessage l_msg;
 
             light_cmd_mutex_.lock();
-            if(light_ctrl_enabled_)
+            if (light_ctrl_enabled_)
             {
                 l_msg.data.cmd.light_ctrl_enable = ENABLE_LIGHT_CTRL;
 
                 l_msg.data.cmd.front_light_mode = static_cast<uint8_t>(current_light_cmd_.front_mode);
                 l_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value;
                 l_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode);
-                l_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;                
+                l_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;
             }
             else
             {
@@ -88,7 +152,7 @@ void ScoutBase::ControlLoop(int32_t period_ms)
                 l_msg.data.cmd.front_light_custom = 0;
                 l_msg.data.cmd.rear_light_mode = CONST_OFF;
                 l_msg.data.cmd.rear_light_custom = 0;
-            } 
+            }
             light_ctrl_requested_ = false;
             light_cmd_mutex_.unlock();
 

scout_base/src/scout_sdk/src/sdk_core/scout_base/tests/CMakeLists.txt

@@ -0,0 +1,9 @@
+# Dependency libraries
+#find_package(LIBRARY_NAME REQUIRED)
+
+# tests
+add_executable(test_scout_base test_scout_base.cpp)
+target_link_libraries(test_scout_base scoutbase)
+
+add_executable(test_can_msg test_can_msg.cpp)
+target_link_libraries(test_can_msg scoutbase)

scout_base/src/scout_sdk/src/sdk_core/scout_base/tests/test_can_msg.cpp

@@ -12,7 +12,7 @@ void print_msg(uint8_t data[8])
 int main()
 {
     MotionControlMessage msg;
-    msg.data.cmd.control_mode = CMD_MODE;
+    msg.data.cmd.control_mode = CMD_CAN_MODE;
     msg.data.cmd.fault_clear_flag = NO_FAULT;
     msg.data.cmd.linear_velocity_cmd = 10;
     msg.data.cmd.angular_velocity_cmd = 0;

scout_base/src/scout_sdk/src/sdk_core/scout_base/tests/test_scout_base.cpp

@@ -8,8 +8,6 @@
 #include <chrono>
 #include <cmath>
 
-#include "stopwatch/stopwatch.h"
-
 #include "scout/scout_base.hpp"
 
 #define TEST_WITHOUT_SERIAL_HARDWARE

scout_base/src/scout_sdk/src/sdk_core/unit_tests/CMakeLists.txt

@@ -6,7 +6,7 @@ set(GTEST_SRC
     gtests/scout_can_protocol_test.cpp
 )
 add_executable(scout_sdk_test ${GTEST_SRC})
-target_link_libraries(scout_sdk_test gtest gtest_main scout_base)
+target_link_libraries(scout_sdk_test gtest gtest_main scoutbase)
 target_include_directories(scout_sdk_test PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<BUILD_INTERFACE:${GTEST_INCLUDE_DIRS}>

scout_base/src/scout_sdk/src/third_party/CMakeLists.txt

@@ -1,14 +1,6 @@
-cmake_minimum_required(VERSION 3.0.0)
-project(scout_thirdparty)
-
-## Put all binary files into /bin and libraries into /lib
-#set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
-set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
-set(ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
-
 ## Add sub source directories
 add_subdirectory(asio)
-add_subdirectory(stopwatch)
+
 if(BUILD_TESTS)
 add_subdirectory(googletest)
 endif()

scout_base/src/scout_sdk/src/third_party/stopwatch/CMakeLists.txt

@@ -1,23 +0,0 @@
-cmake_minimum_required(VERSION 3.0.0)
-project(stopwatch)
-
-## Set compiler to use c++ 11 features
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX14)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
-elseif(COMPILER_SUPPORTS_CXX11)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-else()
-	message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 or C++11 support. Please use a different C++ compiler.")
-endif()
-
-# ascent library
-add_library(stopwatch INTERFACE)
-target_include_directories(stopwatch INTERFACE
-	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-	$<INSTALL_INTERFACE:include>)

scout_base/src/scout_sdk/src/third_party/stopwatch/LICENSE

@@ -1,24 +0,0 @@
-This is free and unencumbered software released into the public domain.
-
-Anyone is free to copy, modify, publish, use, compile, sell, or
-distribute this software, either in source code form or as a compiled
-binary, for any purpose, commercial or non-commercial, and by any
-means.
-
-In jurisdictions that recognize copyright laws, the author or authors
-of this software dedicate any and all copyright interest in the
-software to the public domain. We make this dedication for the benefit
-of the public at large and to the detriment of our heirs and
-successors. We intend this dedication to be an overt act of
-relinquishment in perpetuity of all present and future rights to this
-software under copyright law.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
-EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
-MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
-IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
-OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
-ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
-OTHER DEALINGS IN THE SOFTWARE.
-
-For more information, please refer to <http://unlicense.org>

scout_base/src/scout_sdk/src/third_party/stopwatch/README.md

@@ -1,123 +0,0 @@
-# ⏱️ stopwatch
-Single-header C++11 RDTSCP clock and timing utils released into the public domain.
-
-# why
-While developing games, I have wanted the following features which are not provided by `std::chrono`:
-1. triggering events after a certain amount of time
-2. timing function calls in a high precision manner
-
-# requirements
-1. The `RDTSCP` instruction and a compiler which supports C++11 or higher.
-2. Your processor must have an [Intel Nehalem (2008)](https://en.wikipedia.org/wiki/Nehalem_(microarchitecture)) or newer processor _or_ a processeor with an invariant TSC.
-
-If you do not meet these requirements, you can easily remove the `RDTSCP` code from the library and enjoy the other features. The relevant sections of the [The Intel Software Developer Manuals](http://www.intel.com/Assets/en_US/PDF/manual/253668.pdf) are at the bottom of this page.
-
-# usage
-## timer
-```c++
-#include "stopwatch.h"
-#include <chrono>
-#include <iostream>
-#include <thread>
-
-int main() {
-  const auto timer = stopwatch::make_timer(std::chrono::seconds(10));
-  while (!timer.done()) {
-    std::cout << std::chrono::duration_cast<std::chrono::seconds>(
-                     timer.remaining())
-                     .count()
-              << " seconds remain." << std::endl;
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  }
-  std::cout << "10 seconds have elapsed" << std::endl;
-}
-```
-
-## timing one function call
-```c++
-#include "stopwatch.h"
-#include <iostream>
-
-int main() {
-  const auto cycles = stopwatch::time([] {
-    for (std::size_t i = 0; i < 10; ++i) {
-      std::cout << i << std::endl;
-    }
-  });
-
-  std::cout << "To print out 10 numbers, it took " << cycles.count()
-            << " cycles." << std::endl;
-}
-```
-
-## sampling multiple calls to a function
-Taking the median number of cycles for inserting 10000 items into the beginning of a container.
-```c++
-#include "stopwatch.h"
-#include <deque>
-#include <iostream>
-#include <vector>
-
-int main() {
-  const auto deque_samples = stopwatch::sample<100>([] {
-    std::deque<int> deque;
-    for (std::size_t i = 0; i < 10000; ++i) {
-      deque.insert(deque.begin(), i);
-    }
-  });
-
-  const auto vector_samples = stopwatch::sample<100>([] {
-    std::vector<int> vector;
-    for (std::size_t i = 0; i < 10000; ++i) {
-      vector.insert(vector.begin(), i);
-    }
-  });
-
-  std::cout << "median for deque: " << deque_samples[49].count() << std::endl;
-  std::cout << "median for vector: " << vector_samples[49].count() << std::endl;
-}
-```
-
-Output on my MacbookPro 2016:
-```
-median for deque:   487760
-median for vector: 7595754
-```
-
-# using another clock
-Using another clock is as simple as passing the clock in as a template argument. An example using `std::chrono::system_clock` inplace of `stopwatch::rdtscp_clock` for the `timing one function call` example:
-```c++
-  const auto cycles = stopwatch::time<std::chrono::system_clock>([] {
-    for (std::size_t i = 0; i < 10; ++i) {
-      std::cout << i << std::endl;
-    }
-  });
-```
-`stopwatch::time([] { ... })` became `stopwatch::time<std::chrono::system_clock>([] { ... }`. That's it!
-
-# contributing
-Contributions of any variety are greatly appreciated. All code is passed through `clang-format` using the Google style.
-
-## [The Intel Software Developer Manuals](http://www.intel.com/Assets/en_US/PDF/manual/253668.pdf)
-### Section 16.12.1
-> The time stamp counter in newer processors may support an enhancement, referred
-to as invariant TSC. Processor’s support for invariant TSC is indicated by
-CPUID.80000007H:EDX[8].
-The invariant TSC will run at a constant rate in all ACPI P-, C-. and T-states. This is
-the architectural behavior moving forward. On processors with invariant TSC
-support, the OS may use the TSC for wall clock timer services (instead of ACPI or
-HPET timers). TSC reads are much more efficient and do not incur the overhead
-associated with a ring transition or access to a platform resource.
-
-### Section 16.12.2
-> Processors based on Intel microarchitecture code name Nehalem provide an auxiliary
-TSC register, IA32_TSC_AUX that is designed to be used in conjunction with
-IA32_TSC. IA32_TSC_AUX provides a 32-bit field that is initialized by privileged software
-with a signature value (for example, a logical processor ID).
-
-> The primary usage of IA32_TSC_AUX in conjunction with IA32_TSC is to allow software
-to read the 64-bit time stamp in IA32_TSC and signature value in
-IA32_TSC_AUX with the instruction RDTSCP in an atomic operation. RDTSCP returns
-the 64-bit time stamp in EDX:EAX and the 32-bit TSC_AUX signature value in ECX.
-The atomicity of RDTSCP ensures that no context switch can occur between the reads
-of the TSC and TSC_AUX values.

scout_base/src/scout_sdk/src/third_party/stopwatch/include/stopwatch/stopwatch.h

@@ -1,147 +0,0 @@
-#ifndef STOPWATCH_H_
-#define STOPWATCH_H_
-
-#include <algorithm>
-#include <array>
-#include <chrono>
-#include <cstdint>
-#include <ratio>
-#include <thread>
-
-namespace stopwatch
-{
-// only supported on x86 processors
-#if (defined __x86_64__) || (defined __i386)
-// An implementation of the 'TrivialClock' concept using the rdtscp instruction.
-struct rdtscp_clock
-{
-    using rep = std::uint64_t;
-    using period = std::ratio<1>;
-    using duration = std::chrono::duration<rep, period>;
-    using time_point = std::chrono::time_point<rdtscp_clock, duration>;
-
-    static auto now() noexcept -> time_point
-    {
-        std::uint32_t hi, lo;
-        __asm__ __volatile__("rdtscp"
-                             : "=d"(hi), "=a"(lo));
-        return time_point(duration((static_cast<std::uint64_t>(hi) << 32) | lo));
-    }
-};
-
-// A timer using the specified clock.
-template <class Clock = std::chrono::system_clock>
-struct timer
-{
-    using time_point = typename Clock::time_point;
-    using duration = typename Clock::duration;
-
-    timer(const duration duration) noexcept : expiry(Clock::now() + duration) {}
-    timer(const time_point expiry) noexcept : expiry(expiry) {}
-
-    bool done(time_point now = Clock::now()) const noexcept
-    {
-        return now >= expiry;
-    }
-
-    auto remaining(time_point now = Clock::now()) const noexcept -> duration
-    {
-        return expiry - now;
-    }
-
-    const time_point expiry;
-};
-
-template <class Clock = std::chrono::system_clock>
-constexpr auto make_timer(typename Clock::duration duration) -> timer<Clock>
-{
-    return timer<Clock>(duration);
-}
-
-// Times how long it takes a function to execute using the specified clock.
-template <class Clock = rdtscp_clock, class Func>
-auto time(Func &&function) -> typename Clock::duration
-{
-    const auto start = Clock::now();
-    function();
-    return Clock::now() - start;
-}
-
-// Samples the given function N times using the specified clock.
-template <std::size_t N, class Clock = rdtscp_clock, class Func>
-auto sample(Func &&function) -> std::array<typename Clock::duration, N>
-{
-    std::array<typename Clock::duration, N> samples;
-
-    for (std::size_t i = 0u; i < N; ++i)
-    {
-        samples[i] = time<Clock>(function);
-    }
-
-    std::sort(samples.begin(), samples.end());
-    return samples;
-}
-#endif /* __x86_64__ or __i386 */
-
-struct StopWatch
-{
-    using Clock = std::chrono::high_resolution_clock;
-    using time_point = typename Clock::time_point;
-    using duration = typename Clock::duration;
-
-    StopWatch() { tic_point = Clock::now(); };
-
-    time_point tic_point;
-
-    void tic()
-    {
-        tic_point = Clock::now();
-    };
-
-    double toc()
-    {
-        return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count() / 1000000.0;
-    };
-
-    // for different precisions
-    double stoc()
-    {
-        return std::chrono::duration_cast<std::chrono::seconds>(Clock::now() - tic_point).count();
-    };
-
-    double mtoc()
-    {
-        return std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
-    };
-
-    double utoc()
-    {
-        return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
-    };
-
-    double ntoc()
-    {
-        return std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() - tic_point).count();
-    };
-
-    // you have to call tic() before calling this function
-    void sleep_until_ms(int64_t period_ms)
-    {
-        int64_t duration = period_ms - std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
-
-        if (duration > 0)
-            std::this_thread::sleep_for(std::chrono::milliseconds(duration));
-    };
-
-    void sleep_until_us(int64_t period_us)
-    {
-        int64_t duration = period_us - std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
-
-        if (duration > 0)
-            std::this_thread::sleep_for(std::chrono::microseconds(duration));
-    };
-};
-
-} // namespace stopwatch
-
-#endif // STOPWATCH_H_

scout_bringup/CMakeLists.txt

@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(scout_bringup)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES scout_bringup
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/scout_bringup.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/scout_bringup_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_scout_bringup.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

scout_bringup/launch/scout_minimal.launch

@@ -0,0 +1,3 @@
+<launch>
+    <include file="$(find scout_base)/launch/scout_base.launch" />
+</launch>

scout_bringup/launch/scout_minimal_sim.launch

@@ -0,0 +1,3 @@
+<launch>
+    <include file="$(find scout_base)/launch/scout_base_sim.launch" />
+</launch>

scout_bringup/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>scout_bringup</name>
+  <version>0.0.0</version>
+  <description>The scout_bringup package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="rdu@todo.todo">rdu</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/scout_bringup</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

scout_msgs/CHANGELOG.md

@@ -0,0 +1,6 @@
+# Changelog for package scout_msgs
+
+## 0.0.1 (2019-06-14)
+------------------
+* Initial development of scout_msgs for Scout
+* Contributors: Ruixiang Du

scout_msgs/CMakeLists.txt

@@ -0,0 +1,202 @@
+
+
+cmake_minimum_required(VERSION 2.8.3)
+project(scout_msgs)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  std_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+    FILES
+    ScoutStatus.msg
+    ScoutMotorState.msg
+    ScoutLightState.msg
+    ScoutLightCmd.msg
+)
+
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+catkin_package(CATKIN_DEPENDS std_msgs message_runtime)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES scout_python
+#  CATKIN_DEPENDS rospy scout_msgs std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+#  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/scout_python.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/scout_python_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_scout_python.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

scout_msgs/msg/ScoutLightCmd.msg

@@ -0,0 +1,10 @@
+uint8 LIGHT_CONST_OFF = 0
+uint8 LIGHT_CONST_ON = 1
+uint8 LIGHT_BREATH = 2
+uint8 LIGHT_CUSTOM = 3
+
+bool enable_cmd_light_control
+uint8 front_mode
+uint8 front_custom_value
+uint8 rear_mode
+uint8 rear_custom_value

scout_msgs/msg/ScoutLightState.msg

@@ -0,0 +1,2 @@
+uint8 mode
+uint8 custom_value

scout_msgs/msg/ScoutMotorState.msg

@@ -0,0 +1,3 @@
+float64 current
+float64 rpm
+float64 temperature

scout_msgs/msg/ScoutStatus.msg

@@ -0,0 +1,27 @@
+Header header
+
+int8 MOTOR_ID_FRONT_RIGHT = 0
+int8 MOTOR_ID_FRONT_LEFT = 1
+int8 MOTOR_ID_REAR_RIGHT = 2
+int8 MOTOR_ID_REAR_LEFT = 3
+
+int8 LIGHT_ID_FRONT = 0
+int8 LIGHT_ID_REAR = 1
+
+# motion state
+float64 linear_velocity
+float64 angular_velocity
+
+# base state
+uint8 base_state
+uint8 control_mode
+uint16 fault_code
+float64 battery_voltage
+
+# motor state
+ScoutMotorState[4] motor_states
+
+# light state
+bool light_control_enabled
+ScoutLightState front_light_state
+ScoutLightState rear_light_state

scout_msgs/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package>
+  <name>scout_msgs</name>
+  <version>0.3.3</version>
+  <description>Messages for AgileX Scout</description>
+
+  <author email="contact@westonrobot.com">TODO</author>
+
+  <maintainer email="contact@westonrobot.com">TODO</maintainer>
+  <maintainer email="contact@westonrobot.com">TODO</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">TODO</url>
+  <url type="bugtracker">TODO</url>
+  <url type="repository">TODO</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <export>
+  </export>
+</package>

scout_robot/CHANGELOG.md

@@ -0,0 +1,6 @@
+# Changelog for package scout_robot
+
+## 0.0.1 (2019-05-08)
+------------------
+* Initial development of scout_robot for Scout
+* Contributors: Ruixiang Du

scout_robot/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(scout_robot)
+find_package(catkin REQUIRED)
+catkin_metapackage()

scout_robot/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package>
+  <name>scout_robot</name>
+  <version>0.3.3</version>
+  <description>Metapackage for AgileX Scout robot software</description>
+
+  <maintainer email="ruixiang.du@westonrobot.com">Ruixiang Du</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">TODO</url>
+  <url type="bugtracker">TODO</url>
+  <url type="repository">TODO</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <run_depend>scout_bringup</run_depend>
+  <run_depend>scout_base</run_depend>
+  <run_depend>scout_msgs</run_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>