code compiles with new structure

tests/CMakeLists.txt

@@ -0,0 +1,22 @@
+# Add executables
+add_executable(test_aserial test_aserial.cpp)
+target_link_libraries(test_aserial wrpsdk)
+
+add_executable(test_aserial_comm test_aserial_comm.cpp)
+target_link_libraries(test_aserial_comm wrpsdk)
+
+add_executable(test_asio_can test_asio_can.cpp)
+target_link_libraries(test_asio_can wrpsdk)
+
+add_executable(test_acan test_acan.cpp)
+target_link_libraries(test_acan wrpsdk)
+
+# hunter
+add_executable(test_hunter_base test_hunter_base.cpp)
+target_link_libraries(test_hunter_base wrpsdk)
+
+# scout
+add_executable(test_scout_base test_scout_base.cpp)
+target_link_libraries(test_scout_base wrpsdk)
+
+# tracer

tests/test_acan.cpp

@@ -0,0 +1,60 @@
+#include <iostream>
+#include "wrp_sdk/async_io/async_can.hpp"
+
+using namespace wescore;
+
+void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
+{
+    std::cout << "parser called" << std::endl;
+
+    // mavlink::mavlink_status_t status;
+    // mavlink::mavlink_message_t message;
+
+    for (; bytes_received > 0; bytes_received--)
+    {
+        auto c = *buf++;
+
+        // 	// based on mavlink_parse_char()
+        // 	auto msg_received = static_cast<Framing>(mavlink::mavlink_frame_char_buffer(&m_buffer, &m_status, c, &message, &status));
+        // 	if (msg_received == Framing::bad_crc || msg_received == Framing::bad_signature) {
+        // 		mavlink::_mav_parse_error(&m_status);
+        // 		m_status.msg_received = mavlink::MAVLINK_FRAMING_INCOMPLETE;
+        // 		m_status.parse_state = mavlink::MAVLINK_PARSE_STATE_IDLE;
+        // 		if (c == MAVLINK_STX) {
+        // 			m_status.parse_state = mavlink::MAVLINK_PARSE_STATE_GOT_STX;
+        // 			m_buffer.len = 0;
+        // 			mavlink::mavlink_start_checksum(&m_buffer);
+        // 		}
+        // 	}
+
+        // 	if (msg_received != Framing::incomplete) {
+        // 		log_recv(pfx, message, msg_received);
+
+        // 		if (message_received_cb)
+        // 			message_received_cb(&message, msg_received);
+        // 	}
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    std::shared_ptr<ASyncCAN> canbus = std::make_shared<ASyncCAN>("can1");
+
+    // canbus->set_receive_callback(parse_buffer);
+
+    // if (canbus->is_open())
+    //     std::cout << "can bus connected" << std::endl;
+
+    struct can_frame frame;
+    frame.can_id = 0x123;
+    frame.can_dlc = 2;
+    frame.data[0] = 0x11;
+    frame.data[1] = 0x23;
+
+    while (1)
+    {
+        // canbus->send_bytes(data, 3);
+        canbus->send_frame(frame);
+        sleep(1);
+    }
+}

tests/test_aserial.cpp

@@ -0,0 +1,42 @@
+/*
+ * test_interface.cpp
+ *
+ *  Created on: Dec 25, 2016
+ *      Author: rdu
+ */
+
+#include <iostream>
+
+#include "wrp_sdk/async_io/async_serial.hpp"
+
+using namespace wescore;
+
+void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
+{
+    std::cout << "parser called" << std::endl;
+
+    for (; bytes_received > 0; bytes_received--)
+    {
+        auto c = *buf++;
+
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    // ASyncSerial::Ptr serial = ASyncSerial::open_url("/dev/ttyUSB0:115200");
+    std::shared_ptr<ASyncSerial> serial = std::make_shared<ASyncSerial>("/dev/ttyO5", 115200);
+
+    serial->set_receive_callback(parse_buffer);
+
+    if (serial->is_open())
+        std::cout << "serial port opened" << std::endl;
+
+    uint8_t data[8] = {'a','b','c'};
+
+    while (1)
+    {
+        // serial->send_bytes(data, 3);
+        sleep(1);
+    }
+}

tests/test_aserial_comm.cpp

@@ -0,0 +1,82 @@
+/*
+ * test_interface.cpp
+ *
+ *  Created on: Dec 25, 2016
+ *      Author: rdu
+ */
+
+#include <iostream>
+
+#include "wrp_sdk/async_io/async_serial.hpp"
+
+using namespace wescore;
+
+void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
+{
+    // std::cout << "parser called" << std::endl;
+
+    // for (int i = 0; i < bytes_received; ++i)
+    // {
+    //     // auto c = *buf++;
+    //     std::cout << std::hex << static_cast<int>(buf[i]) << std::dec << " ";
+    // }
+
+    if (bytes_received > 2)
+    {
+        for (int i = 0; i < bytes_received - 1; ++i)
+        {
+            uint8_t first = buf[i];
+            uint8_t second = buf[i + 1];
+
+            if (first == 0xB5 && second == 0x62)
+                std::cout << "- start bytes found" << std::endl;
+            // std::cout << std::hex << static_cast<int>(buf[i]) << std::dec << " ";
+        }
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    std::string device_name;
+    int baud;
+
+    if (argc == 3)
+    {
+        device_name = {argv[1]};
+        baud = std::stoi(argv[2]);
+        std::cout << "Specified device: " << device_name << ", baud: " << baud << std::endl;
+    }
+    else
+    {
+        std::cout << "Usage: test_aserial_comm <interface> <baud>" << std::endl
+                  << "Example: ./test_aserial_comm /dev/ttyUSB0 115200" << std::endl;
+        return -1;
+    }
+
+    std::shared_ptr<ASyncSerial> serial = std::make_shared<ASyncSerial>(device_name, baud);
+
+    serial->set_receive_callback(parse_buffer);
+
+    if (serial->is_open())
+        std::cout << "serial port opened" << std::endl;
+
+    uint8_t data[8] = {'a', 'b', 'c'};
+
+    int count = 0;
+    uint8_t idx = 0;
+    const unsigned baudrates[] = {9600, 57600, 115200};
+    while (1)
+    {
+        // serial->send_bytes(data, 3);
+        if (++count == 15)
+        {
+            count = 0;
+            std::cout << "----------------------------------------" << std::endl;
+            std::cout << "change baud rate to " << baudrates[idx % 3] << std::endl;
+            std::cout << "----------------------------------------" << std::endl;
+            serial->set_baud(baudrates[idx % 3]);
+            idx++;
+        }
+        sleep(1);
+    }
+}

tests/test_asio_can.cpp

@@ -0,0 +1,79 @@
+// source: https://stackoverflow.com/questions/10467178/boostasio-over-socketcan
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <iostream>
+
+#include <net/if.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+#define ASIO_ENABLE_OLD_SERVICES
+#define ASIO_HAS_POSIX_STREAM_DESCRIPTOR
+
+#include "asio.hpp"
+#include <functional>
+
+#include "asio/posix/basic_stream_descriptor.hpp"
+
+void data_send(void)
+{
+    std::cout << "omg sent" << std::endl;
+}
+
+void data_rec(struct can_frame &rec_frame,
+              asio::posix::basic_stream_descriptor<> &stream)
+{
+    std::cout << std::hex << rec_frame.can_id << "  ";
+    for (int i = 0; i < rec_frame.can_dlc; i++)
+    {
+        std::cout << std::hex << int(rec_frame.data[i]) << " ";
+    }
+    std::cout << std::dec << std::endl;
+    stream.async_read_some(
+        asio::buffer(&rec_frame, sizeof(rec_frame)),
+        std::bind(data_rec, std::ref(rec_frame), std::ref(stream)));
+}
+
+int main(void)
+{
+    struct sockaddr_can addr;
+    struct can_frame frame;
+    struct can_frame rec_frame;
+    struct ifreq ifr;
+
+    int natsock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+
+    strcpy(ifr.ifr_name, "can1");
+    ioctl(natsock, SIOCGIFINDEX, &ifr);
+
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+    if (bind(natsock, (struct sockaddr *)&addr, sizeof(addr)) < 0)
+    {
+        perror("Error in socket bind");
+        return -2;
+    }
+
+    frame.can_id = 0x123;
+    frame.can_dlc = 2;
+    frame.data[0] = 0x11;
+    frame.data[1] = 0x23;
+
+    asio::io_service ios;
+    asio::posix::basic_stream_descriptor<> stream(ios);
+    stream.assign(natsock);
+
+    stream.async_write_some(asio::buffer(&frame, sizeof(frame)),
+                            std::bind(data_send));
+    stream.async_read_some(
+        asio::buffer(&rec_frame, sizeof(rec_frame)),
+        std::bind(data_rec, std::ref(rec_frame), std::ref(stream)));
+    ios.run();
+}

tests/test_can_msg.cpp

@@ -0,0 +1,39 @@
+#include <iostream>
+
+#include "scout_base/details/scout_can_parser.hpp"
+
+using namespace wescore;
+
+void print_msg(uint8_t data[8])
+{
+    for (int i = 0; i < 8; ++i)
+        std::cout << std::hex << static_cast<int>(data[i]) << " ";
+    std::cout << std::dec << std::endl;
+}
+
+int main()
+{
+    MotionControlMessage msg;
+    msg.msg.cmd.control_mode = CTRL_MODE_CMD_CAN;
+    msg.msg.cmd.fault_clear_flag = FAULT_CLR_NONE;
+    msg.msg.cmd.linear_velocity_cmd = 10;
+    msg.msg.cmd.angular_velocity_cmd = 0;
+    msg.msg.cmd.reserved0 = 0;
+    msg.msg.cmd.reserved1 = 0;
+    msg.msg.cmd.count = 0;
+    msg.msg.cmd.checksum = ScoutCANParser::Agilex_CANMsgChecksum(ScoutCANParser::CAN_MSG_MOTION_CONTROL_CMD_ID, msg.msg.raw, msg.len);
+    print_msg(msg.msg.raw);
+
+    LightControlMessage msg2;
+    msg2.msg.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL;
+    msg2.msg.cmd.front_light_mode = LIGHT_MODE_CONST_ON;
+    msg2.msg.cmd.front_light_custom = 0;
+    msg2.msg.cmd.rear_light_mode = LIGHT_MODE_CONST_ON;
+    msg2.msg.cmd.rear_light_custom = 0;
+    msg2.msg.cmd.reserved0 = 0;
+    msg2.msg.cmd.count = 0;
+    msg2.msg.cmd.checksum = ScoutCANParser::Agilex_CANMsgChecksum(ScoutCANParser::CAN_MSG_LIGHT_CONTROL_CMD_ID, msg2.msg.raw, msg2.len);
+    print_msg(msg2.msg.raw);
+
+    return 0;
+}

tests/test_hunter_base.cpp

@@ -0,0 +1,54 @@
+#include <unistd.h>
+
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <iostream>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#include "wrp_sdk/platforms/hunter/hunter_base.hpp"
+
+#define TEST_WITHOUT_SERIAL_HARDWARE
+
+using namespace wescore;
+
+int main(int argc, char **argv) {
+  std::string device_name;
+  int32_t baud_rate = 0;
+
+  if (argc == 2) {
+    device_name = {argv[1]};
+    std::cout << "Specified CAN: " << device_name << std::endl;
+  } else {
+    std::cout << "Usage: app_scout_demo <interface>" << std::endl
+              << "Example 1: ./app_scout_demo can0" << std::endl;
+    return -1;
+  }
+
+  HunterBase scout;
+  scout.Connect(device_name);
+
+  int count = 0;
+  while (true) {
+    std::cout << "Motor: 0.0, 0.0," << std::endl;
+    scout.SetMotionCommand(0.0, 0.0);
+
+    auto state = scout.GetHunterState();
+    std::cout << "-------------------------------" << std::endl;
+    std::cout << "count: " << count << std::endl;
+    std::cout << "control mode: " << static_cast<int>(state.control_mode)
+              << " , base state: " << static_cast<int>(state.base_state)
+              << std::endl;
+    std::cout << "battery voltage: " << state.battery_voltage << std::endl;
+    std::cout << "velocity (linear, angular): " << state.linear_velocity << ", "
+              << state.steering_angle << std::endl;
+    std::cout << "-------------------------------" << std::endl;
+
+    sleep(1);
+    ++count;
+  }
+
+  return 0;
+}

tests/test_scout_base.cpp

@@ -0,0 +1,69 @@
+#include <unistd.h>
+
+#include <thread>
+#include <mutex>
+#include <functional>
+#include <string>
+#include <iostream>
+#include <chrono>
+#include <cmath>
+
+#include "wrp_sdk/platforms/scout/scout_base.hpp"
+
+#define TEST_WITHOUT_SERIAL_HARDWARE
+
+using namespace wescore;
+
+int main(int argc, char **argv)
+{
+    std::string device_name;
+    int32_t baud_rate = 0;
+
+    if (argc == 2)
+    {
+        device_name = {argv[1]};
+        std::cout << "Specified CAN: " << device_name << std::endl;
+    }
+    else if (argc == 3)
+    {
+        device_name = {argv[1]};
+        baud_rate = std::stol(argv[2]);
+        std::cout << "Specified serial: " << device_name << "@" << baud_rate << std::endl;
+    }
+    else
+    {
+        std::cout << "Usage: app_scout_monitor <interface>" << std::endl
+                  << "Example 1: ./app_scout_monitor can1"
+                  << "Example 2: ./app_scout_monitor /dev/ttyUSB0 115200" << std::endl;
+        return -1;
+    }
+
+    ScoutBase scout;
+    scout.Connect(device_name, baud_rate);
+
+    scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0});
+
+    int count = 0;
+    while (true)
+    {
+        scout.SetMotionCommand(0.5, 0.2);
+
+        if(count == 10)
+        {
+            // scout.SetLightCommand({ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0, ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0});
+            scout.DisableLightCmdControl();
+        }
+
+        auto state = scout.GetScoutState();
+        std::cout << "-------------------------------" << std::endl;
+        std::cout << "control mode: " << static_cast<int>(state.control_mode) << " , base state: " << static_cast<int>(state.base_state) << std::endl;
+        std::cout << "battery voltage: " << state.battery_voltage << std::endl;
+        std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl;
+        std::cout << "-------------------------------" << std::endl;
+
+        sleep(1);
+        ++count;
+    }
+
+    return 0;
+}

tests/test_scout_serial.cpp

@@ -0,0 +1,45 @@
+#include <iostream>
+
+#include "scout_base/details/scout_serial_parser.hpp"
+
+using namespace wescore;
+
+#include "scout_base/scout_base.hpp"
+
+#define TEST_WITHOUT_SERIAL_HARDWARE
+
+using namespace wescore;
+
+int main(int argc, char **argv)
+{
+    ScoutBase scout;
+    // scout.ConfigureCANBus("can1");
+    scout.Connect("/dev/ttyUSB0", 115200);
+    // scout.StartCmdThread(10);
+
+    // scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0});
+
+    int count = 0;
+    while (true)
+    {
+        // scout.SetMotionCommand(0.5, 0.2);
+
+        // if(count == 10)
+        // {
+        //     // scout.SetLightCommand({ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0, ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0});
+        //     scout.DisableLightCmdControl();
+        // }
+
+        auto state = scout.GetScoutState();
+        std::cout << "-------------------------------" << std::endl;
+        std::cout << "control mode: " << static_cast<int>(state.control_mode) << " , base state: " << static_cast<int>(state.base_state) << std::endl;
+        std::cout << "battery voltage: " << state.battery_voltage << std::endl;
+        std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl;
+        std::cout << "-------------------------------" << std::endl;
+
+        sleep(1);
+        ++count;
+    }
+
+    return 0;
+}

tests/test_serial_parser.cpp

@@ -0,0 +1,51 @@
+#include <iostream>
+
+#include "scout_base/details/scout_serial_parser.hpp"
+
+using namespace wescore;
+
+void print_msg(uint8_t data[8])
+{
+    for (int i = 0; i < 8; ++i)
+        std::cout << std::hex << static_cast<int>(data[i]) << " ";
+    std::cout << std::dec << std::endl;
+}
+
+uint8_t calc_checksum(uint8_t *buf, uint8_t len)
+{
+    uint8_t checksum = 0;
+    for (int i = 0; i < len; ++i)
+        checksum ^= buf[i];
+    return checksum;
+}
+
+int main()
+{
+    uint8_t frame_data[16];
+
+    // SOF
+    frame_data[0] = 0x5a;
+    frame_data[1] = 0xa5;
+
+    // Frame len, type, ID
+    frame_data[2] = 0x0a;
+    frame_data[3] = 0xaa;
+    frame_data[4] = 0x01;
+
+    // Frame payload
+    frame_data[5] = 0;
+    frame_data[6] = 1;
+    frame_data[7] = 2;
+    frame_data[8] = 3;
+    frame_data[9] = 4;
+    frame_data[10] = 5;
+
+    // Frame count, checksum
+    frame_data[11] = 1;
+    frame_data[12] = calc_checksum(frame_data, 12);
+
+    ScoutSerialParser parser;
+    parser.ParseBuffer(frame_data, 7 + 6);
+
+    return 0;
+}

tests/test_tracer_base.cpp

@@ -0,0 +1,69 @@
+#include <unistd.h>
+
+#include <thread>
+#include <mutex>
+#include <functional>
+#include <string>
+#include <iostream>
+#include <chrono>
+#include <cmath>
+
+#include "scout_base/scout_base.hpp"
+
+#define TEST_WITHOUT_SERIAL_HARDWARE
+
+using namespace wescore;
+
+int main(int argc, char **argv)
+{
+    std::string device_name;
+    int32_t baud_rate = 0;
+
+    if (argc == 2)
+    {
+        device_name = {argv[1]};
+        std::cout << "Specified CAN: " << device_name << std::endl;
+    }
+    else if (argc == 3)
+    {
+        device_name = {argv[1]};
+        baud_rate = std::stol(argv[2]);
+        std::cout << "Specified serial: " << device_name << "@" << baud_rate << std::endl;
+    }
+    else
+    {
+        std::cout << "Usage: app_scout_monitor <interface>" << std::endl
+                  << "Example 1: ./app_scout_monitor can1"
+                  << "Example 2: ./app_scout_monitor /dev/ttyUSB0 115200" << std::endl;
+        return -1;
+    }
+
+    ScoutBase scout;
+    scout.Connect(device_name, baud_rate);
+
+    scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0});
+
+    int count = 0;
+    while (true)
+    {
+        scout.SetMotionCommand(0.5, 0.2);
+
+        if(count == 10)
+        {
+            // scout.SetLightCommand({ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0, ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0});
+            scout.DisableLightCmdControl();
+        }
+
+        auto state = scout.GetScoutState();
+        std::cout << "-------------------------------" << std::endl;
+        std::cout << "control mode: " << static_cast<int>(state.control_mode) << " , base state: " << static_cast<int>(state.base_state) << std::endl;
+        std::cout << "battery voltage: " << state.battery_voltage << std::endl;
+        std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl;
+        std::cout << "-------------------------------" << std::endl;
+
+        sleep(1);
+        ++count;
+    }
+
+    return 0;
+}