mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

saved work

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-08 16:22:41 +08:00
parent 2a67bbceb4
commit d2f210e6c1
56 changed files with 777 additions and 2259 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
CMakeLists.txt
View File

@@ -71,21 +71,26 @@ find_package(Threads REQUIRED)
# add sdk main library # add sdk main library
add_library(${PROJECT_NAME} add_library(${PROJECT_NAME}
# async_port from weston robot ########################
## async_port from weston robot
src/async_port/async_serial.cpp src/async_port/async_serial.cpp
src/async_port/async_can.cpp src/async_port/async_can.cpp
# agx common
src/agilex_base.cpp
src/agx_msg_parser.c
# robot support
src/scout_base.cpp
# src/hunter_base.cpp
src/tracer_base.cpp
# src/bunker_base.cpp
src/ranger_base.cpp
######################## ########################
# legacy support ## public interface to access robot
src/legacy/scout_can_parser_v1.c # src/mobile_base/scout_robot.cpp
########################
## protocol v2 support
# parser
# src/protocol_v2/agilex_base.cpp
src/protocol_v2/agx_msg_parser.c
src/protocol_v2/protocol_v2_parser.cpp
# robots
src/protocol_v2/scout_base.cpp
# src/protocol_v2/tracer_base.cpp
# src/protocol_v2/ranger_base.cpp
########################
## legacy protocol v1 support
src/protocol_v1/scout_can_parser_v1.c
) )
target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads) target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads)
target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES) target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES)
@@ -108,7 +113,7 @@ if(BUILD_WITHOUT_ROS)
endif() endif()
# add app source directory # add app source directory
add_subdirectory(apps) add_subdirectory(demo)
endif() endif()
# Build tests # Build tests

17
README.md
View File

@@ -32,12 +32,17 @@ It should also work in other similar Linux environments but only the above liste
**Communication protocol** **Communication protocol**
| Robot | Protocol Version | | Robot | Protocol V1 | Protocol V2 | UART | CAN | Support Status |
| :----: | :--------------: | | :---------------: | :---------: | :---------: | :---: | :---: | :------------: |
| Scout | V1, V2 | | Scout 1.0 | Y | - | Y | Y | Discontinued |
| Hunter | V1, V2 | | Scout 2.0 | Y | Y | Y | Y | Active |
| Tracer | V2 | | Scout Mini (Skid) | Y | Y | - | Y | Active |
| Bunker | V1 | | Scout Mini (Omni) | Y | Y | - | Y | Active |
| Hunter 1.0 | Y | Y | - | Y | Active |
| Hunter 2.0 | - | Y | - | Y | Active |
| Bunker | Y | Y | - | Y | Active |
| Tracer | - | Y | Y | Y | Active |
| Ranger Mini | - | Y | - | Y | Active |
Generally, you only need to instantiate an object of the robot base class (such as ScoutBase), then use the object to programmatically control the robot. Internally, the base class manages two background threads, one to process CAN/UART messages of the robot state and accordingly update state variables in the robot state data structure, and the other to maintain a 50Hz loop and send the latest command to the robot base. User can iteratively perform tasks in the main thread and check the robot state or set control commands. Advanced users may also use this setup as a reference and maintain the control and monitoring loops in a different way. Generally, you only need to instantiate an object of the robot base class (such as ScoutBase), then use the object to programmatically control the robot. Internally, the base class manages two background threads, one to process CAN/UART messages of the robot state and accordingly update state variables in the robot state data structure, and the other to maintain a 50Hz loop and send the latest command to the robot base. User can iteratively perform tasks in the main thread and check the robot state or set control commands. Advanced users may also use this setup as a reference and maintain the control and monitoring loops in a different way.

22
apps/CMakeLists.txt
View File

@@ -1,22 +0,0 @@
# Dependency libraries
#find_package(LIBRARY_NAME REQUIRED)
# tests
# add_executable(app_scout_demo scout_demo/scout_demo.cpp)
# target_link_libraries(app_scout_demo ugv_sdk)
# add_executable(app_scout_discharge scout_demo/scout_discharge.cpp)
# target_link_libraries(app_scout_discharge ugv_sdk)
# add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp)
# target_link_libraries(app_hunter_demo ugv_sdk)
add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp)
target_link_libraries(app_tracer_demo ugv_sdk)
add_executable(app_ranger_demo ranger_demo/ranger_demo.cpp)
target_link_libraries(app_ranger_demo ugv_sdk)
# if(BUILD_MONITOR)
# add_subdirectory(scout_monitor)
# endif()

117
apps/scout_demo/scout_demo.cpp
View File

@@ -1,117 +0,0 @@
/*
* demo_scout_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/scout_base.hpp"
using namespace westonrobot;
int main(int argc, char **argv) {
std::string device_name;
if (argc == 2) {
device_name = {argv[1]};
std::cout << "Specified CAN: " << device_name << std::endl;
} else {
std::cout << "Usage: app_scout_demo <interface>" << std::endl
<< "Example 1: ./app_scout_demo can0" << std::endl;
return -1;
}
ScoutBase scout;
scout.Connect(device_name);
scout.EnableCommandedMode();
// // light control
std::cout << "Light: const off" << std::endl;
scout.SetLightCommand({CONST_OFF, 0,
CONST_OFF, 0});
sleep(3);
std::cout << "Light: const on" << std::endl;
scout.SetLightCommand({CONST_ON, 0,
CONST_ON, 0});
sleep(3);
std::cout << "Light: breath" << std::endl;
scout.SetLightCommand({BREATH, 0,
BREATH, 0});
sleep(3);
std::cout << "Light: custom 90-80" << std::endl;
scout.SetLightCommand({CUSTOM, 90,
CUSTOM, 80});
sleep(3);
std::cout << "Light: diabled cmd control" << std::endl;
scout.DisableLightControl();
int count = 0;
while (true) {
// motion control
// if (count < 5)
// {
// std::cout << "Motor: 0.2, 0" << std::endl;
// scout.SetMotionCommand(0.2, 0.0);
// }
// else if (count < 10)
// {
// std::cout << "Motor: 0.8, 0" << std::endl;
// scout.SetMotionCommand(0.8, 0.0);
// }
// else if (count < 15)
// {
// std::cout << "Motor: 1.5, 0" << std::endl;
// scout.SetMotionCommand(1.5, 0.0);
// }
// else if (count < 20)
// {
// std::cout << "Motor: 1.0, 0.5" << std::endl;
// scout.SetMotionCommand(1.0, 0.5);
// }
// else if (count < 25)
// {
// std::cout << "Motor: 0.0, 0" << std::endl;
// scout.SetMotionCommand(0.0, 0.0);
// }
// else if (count < 30)
// {
// std::cout << "Motor: -0.5, 0" << std::endl;
// scout.SetMotionCommand(-0.5, 0.0);
// }
// else if (count < 35)
// {
// std::cout << "Motor: -1.0, -0.5" << std::endl;
// scout.SetMotionCommand(-1.0, -0.5);
// }
// else if (count < 40)
// {
// std::cout << "Motor: 0.0, 0, Light: breath" << std::endl;
// scout.SetMotionCommand(0.0, 0.0);
// scout.SetLightCommand({BREATH, 0,
// BREATH, 0});
// }
if (count < 100) {
std::cout << "Motor: 0.2, 0" << std::endl;
scout.SetMotionCommand(0.2, 0.0);
}
auto state = scout.GetScoutState();
std::cout << "-------------------------------" << std::endl;
std::cout << "count: " << count << std::endl;
std::cout << "control mode: " << static_cast<int>(state.system_state.control_mode)
<< " , vehicle state: " << static_cast<int>(state.system_state.vehicle_state)
<< std::endl;
std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl;
std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", "
<< state.motion_state.angular_velocity << std::endl;
std::cout << "-------------------------------" << std::endl;
usleep(20000);
++count;
}
return 0;
}

69
apps/scout_demo/scout_discharge.cpp
View File

@@ -1,69 +0,0 @@
/*
* demo_scout_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/scout_base.hpp"
using namespace westonrobot;
int main(int argc, char **argv)
{
std::string device_name;
if (argc == 2)
{
device_name = {argv[1]};
std::cout << "Specified CAN: " << device_name << std::endl;
}
else
{
std::cout << "Usage: app_scout_monitor <interface>" << std::endl
<< "Example 1: ./app_scout_demo can0" << std::endl
<< "Example 2: ./app_scout_demo /dev/ttyUSB0 115200" << std::endl;
return -1;
}
ScoutBase scout;
scout.Connect(device_name);
scout.EnableCommandedMode();
// light control
std::cout << "Light: const on" << std::endl;
scout.SetLightCommand({CONST_ON, 0, CONST_ON, 0});
int count = 0;
while (true)
{
auto state = scout.GetScoutState();
if (state.system_state.battery_voltage >= 22.5)
{
scout.SetMotionCommand(1.35, 0);
std::cout << "-------------------------------" << std::endl;
std::cout << "-------> discharging <--------" << std::endl;
std::cout << "elapsed time: " << count / 60 << " minutes " << count % 60 << " seconds" << std::endl;
std::cout << "control mode: " << static_cast<int>(state.system_state.control_mode) << " , vehicle state: " << static_cast<int>(state.system_state.vehicle_state) << std::endl;
std::cout << "battery voltage: " << state.system_state.battery_voltage << std::endl;
std::cout << "velocity (linear, angular): " << state.motion_state.linear_velocity << ", " << state.motion_state.angular_velocity << std::endl;
std::cout << "-------------------------------" << std::endl;
}
else
{
scout.SetMotionCommand(0, 0);
std::cout << "-------------------------------" << std::endl;
std::cout << "discharge stopped at: " << state.system_state.battery_voltage << " V" << std::endl;
std::cout << "-------------------------------" << std::endl;
}
sleep(1);
++count;
}
return 0;
}

24
apps/scout_monitor/CMakeLists.txt
View File

@@ -1,24 +0,0 @@
# 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 ugv_sdk ${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()

3
apps/scout_monitor/app/CMakeLists.txt
View File

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

40
apps/scout_monitor/app/app_scout_monitor.cpp
View File

@@ -1,40 +0,0 @@
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <string>
#include <iostream>
#include "monitor/scout_monitor.hpp"
using namespace westonrobot;
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" << std::endl
<< "Example 2: ./app_scout_monitor /dev/ttyUSB0 115200" << std::endl;
return -1;
}
ScoutMonitor monitor;
monitor.Run(device_name, baud_rate);
return 0;
}

BIN
apps/scout_monitor/color_demo.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 85 KiB

67
apps/scout_monitor/include/monitor/ncolors.hpp
View File

@@ -1,67 +0,0 @@
/*
* 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 westonrobot
{
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 westonrobot
#endif /* NCOLORS_HPP */

24
apps/scout_monitor/include/monitor/nshapes.hpp
View File

@@ -1,24 +0,0 @@
/*
* 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 westonrobot
{
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 westonrobot
#endif /* NSHAPES_HPP */

78
apps/scout_monitor/include/monitor/scout_monitor.hpp
View File

@@ -1,78 +0,0 @@
/*
* 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 "ugv_sdk/scout_base.hpp"
#include <ncurses.h>
namespace westonrobot
{
class ScoutMonitor
{
public:
ScoutMonitor();
~ScoutMonitor();
void Run(std::string device_name = "", int32_t baud_rate = 0);
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 westonrobot
#endif /* SCOUT_MONITOR_HPP */

116
apps/scout_monitor/src/ncolors.cpp
View File

@@ -1,116 +0,0 @@
/*
* 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);
default:
return COLOR_BLACK;
}
}
} // namespace
namespace westonrobot
{
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 westonrobot

41
apps/scout_monitor/src/nshapes.cpp
View File

@@ -1,41 +0,0 @@
/*
* nshapers.cpp
*
* Created on: Jun 20, 2019 06:21
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "monitor/nshapes.hpp"
namespace westonrobot
{
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 westonrobot

658
apps/scout_monitor/src/scout_monitor.cpp
View File

@@ -1,658 +0,0 @@
/*
* 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 "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 westonrobot
{
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_);
scout_state_.linear_velocity = 0;
scout_state_.angular_velocity = 0;
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_STATE_NORMAL;
scout_state_.battery_voltage = 28.5;
scout_state_.linear_velocity = 1.234;
scout_state_.angular_velocity = -0.6853;
// scout_state_.fault_code |= FAULT_MOTOR_DRV_OVERHEAT_W;
// scout_state_.fault_code |= FAULT_MOTOR_OVERCURRENT_W;
// scout_state_.fault_code |= FAULT_MOTOR_DRV_OVERHEAT_F;
// scout_state_.fault_code |= FAULT_MOTOR_OVERCURRENT_F;
// scout_state_.fault_code |= FAULT_BAT_UNDER_VOL_W;
// scout_state_.fault_code |= FAULT_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 = LIGHT_MODE_CUSTOM;
scout_state_.front_light_state.custom_value = 50;
scout_state_.rear_light_state.mode = LIGHT_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, int32_t baud_rate)
{
if (device_name != "")
test_mode_ = false;
if (test_mode_)
SetTestStateData();
else
scout_base_.Connect(device_name, baud_rate);
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 == LIGHT_MODE_CONST_ON)
front_mode_str += "ON";
else if (scout_state_.front_light_state.mode == LIGHT_MODE_CONST_OFF)
front_mode_str += "OFF";
else if (scout_state_.front_light_state.mode == LIGHT_MODE_BREATH)
front_mode_str += "BREATH";
else if (scout_state_.front_light_state.mode == LIGHT_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 != LIGHT_MODE_CONST_OFF &&
!(scout_state_.front_light_state.mode == LIGHT_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 == LIGHT_MODE_CONST_ON)
rear_mode_str += "ON";
else if (scout_state_.rear_light_state.mode == LIGHT_MODE_CONST_OFF)
rear_mode_str += "OFF";
else if (scout_state_.rear_light_state.mode == LIGHT_MODE_BREATH)
rear_mode_str += "BREATH";
else if (scout_state_.rear_light_state.mode == LIGHT_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 != LIGHT_MODE_CONST_OFF &&
!(scout_state_.rear_light_state.mode == LIGHT_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_STATE_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_STATE_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_STATE_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 == CTRL_MODE_REMOTE)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "REMOTE");
else if (scout_state_.control_mode == CTRL_MODE_CMD_CAN)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CMD");
else if (scout_state_.control_mode == CTRL_MODE_CMD_UART)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CMD");
// 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 & FAULT_MOTOR_DRV_OVERHEAT_W) == 0 &&
(scout_state_.fault_code & FAULT_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 & FAULT_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 & FAULT_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 & FAULT_MOTOR_OVERCURRENT_W) == 0 &&
(scout_state_.fault_code & FAULT_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 & FAULT_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 & FAULT_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 & FAULT_BAT_UNDER_VOL_W) == 0 &&
(scout_state_.fault_code & FAULT_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 & FAULT_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 & FAULT_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 & FAULT_BAT_OVER_VOL_F) == 0);
ShowStatusItemName(sec2 + 5, state_title_col, "-RC signal loss");
ShowFault(sec2 + 5, fault_col_1, (scout_state_.fault_code & FAULT_RC_SIGNAL_LOSS) == 0);
const int sec3 = sec2 + 7;
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 & FAULT_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 & FAULT_MOTOR1_COMM_F) == 0);
ShowStatusItemName(sec3 + 3, state_title_col, "-Motor 2 comm");
ShowFault(sec3 + 3, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR2_COMM_F) == 0);
ShowStatusItemName(sec3 + 4, state_title_col, "-Motor 3 comm");
ShowFault(sec3 + 4, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR3_COMM_F) == 0);
ShowStatusItemName(sec3 + 5, state_title_col, "-Motor 4 comm");
ShowFault(sec3 + 5, fault_col_1, (scout_state_.fault_code & FAULT_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 westonrobot

15
apps/scout_monitor/tests/CMakeLists.txt
View File

@@ -1,15 +0,0 @@
# 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)

158
apps/scout_monitor/tests/test_ncolor.c
View File

@@ -1,158 +0,0 @@
/* 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);
}

65
apps/scout_monitor/tests/test_ncolor2.cpp
View File

@@ -1,65 +0,0 @@
#include "monitor/ncolors.hpp"
using namespace westonrobot;
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;
}

118
apps/scout_monitor/tests/test_ncurses.cpp
View File

@@ -1,118 +0,0 @@
#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();
}

33
apps/scout_monitor/tests/test_scout_monitor.cpp
View File

@@ -1,33 +0,0 @@
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <iostream>
#include "monitor/scout_monitor.hpp"
using namespace westonrobot;
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;
}

33
apps/scout_monitor/tests/test_scout_monitor_virtual.cpp
View File

@@ -1,33 +0,0 @@
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <iostream>
#include "monitor/scout_monitor.hpp"
using namespace westonrobot;
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;
}

11
demo/CMakeLists.txt Executable file
View File

@@ -0,0 +1,11 @@
# demo
add_subdirectory(scout_demo)
# add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp)
# target_link_libraries(app_hunter_demo ugv_sdk)
# add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp)
# target_link_libraries(app_tracer_demo ugv_sdk)
# add_executable(app_ranger_demo ranger_demo/ranger_demo.cpp)
# target_link_libraries(app_ranger_demo ugv_sdk)

0
apps/hunter_demo/hunter_demo.cpp → demo/hunter_demo/hunter_demo.cpp
View File

2
apps/ranger_demo/ranger_demo.cpp → demo/ranger_demo/ranger_demo.cpp
View File

@@ -7,7 +7,7 @@
* Copyright : AgileX Robotics * Copyright : AgileX Robotics
**/ **/
#include "ugv_sdk/ranger_base.hpp" #include "ugv_sdk/protocol_v2/ranger_base.hpp"
using namespace westonrobot; using namespace westonrobot;

6
demo/scout_demo/CMakeLists.txt Executable file
View File

@@ -0,0 +1,6 @@
# tests
add_executable(demo_scout_v2 scout_v2_demo.cpp)
target_link_libraries(demo_scout_v2 ugv_sdk)
# add_executable(demo_scout_robot scout_robot_demo.cpp)
# target_link_libraries(demo_scout_robot ugv_sdk)

78
demo/scout_demo/scout_robot_demo.cpp Normal file
View File

@@ -0,0 +1,78 @@
/*
* scout_robot_demo.cpp
*
* Created on: Jul 08, 2021 11:12
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include <iostream>
#include "ugv_sdk/mobile_base/scout_robot.hpp"
using namespace westonrobot;
int main(int argc, char **argv) {
std::string device_name;
if (argc == 2) {
device_name = {argv[1]};
std::cout << "Specified CAN: " << device_name << std::endl;
} else {
std::cout << "Usage: app_scout_demo <interface>" << std::endl
<< "Example 1: ./app_scout_demo can0" << std::endl;
return -1;
}
ScoutRobot scout(AgilexProtocol::V1);
// scout.Connect(device_name);
// scout.EnableCommandedMode();
// // // light control
// std::cout << "Light: const off" << std::endl;
// scout.SetLightCommand({CONST_OFF, 0, CONST_OFF, 0});
// sleep(3);
// std::cout << "Light: const on" << std::endl;
// scout.SetLightCommand({CONST_ON, 0, CONST_ON, 0});
// sleep(3);
// std::cout << "Light: breath" << std::endl;
// scout.SetLightCommand({BREATH, 0, BREATH, 0});
// sleep(3);
// std::cout << "Light: custom 90-80" << std::endl;
// scout.SetLightCommand({CUSTOM, 90, CUSTOM, 80});
// sleep(3);
// std::cout << "Light: diabled cmd control" << std::endl;
// scout.DisableLightControl();
// int count = 0;
// while (true) {
// // motion control
// if (count < 100) {
// std::cout << "Motor: 0.2, 0" << std::endl;
// scout.SetMotionCommand(0.2, 0.0);
// }
// // get robot state
// auto state = scout.GetScoutState();
// std::cout << "-------------------------------" << std::endl;
// std::cout << "count: " << count << std::endl;
// std::cout << "control mode: "
// << static_cast<int>(state.system_state.control_mode)
// << " , vehicle state: "
// << static_cast<int>(state.system_state.vehicle_state)
// << std::endl;
// std::cout << "battery voltage: " << state.system_state.battery_voltage
// << std::endl;
// std::cout << "velocity (linear, angular): "
// << state.motion_state.linear_velocity << ", "
// << state.motion_state.angular_velocity << std::endl;
// std::cout << "-------------------------------" << std::endl;
// usleep(20000);
// ++count;
// }
return 0;
}

75
demo/scout_demo/scout_v2_demo.cpp Normal file
View File

@@ -0,0 +1,75 @@
/*
* demo_scout_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/protocol_v2/scout_base_v2.hpp"
using namespace westonrobot;
int main(int argc, char **argv) {
std::string device_name;
if (argc == 2) {
device_name = {argv[1]};
std::cout << "Specified CAN: " << device_name << std::endl;
} else {
std::cout << "Usage: app_scout_demo <interface>" << std::endl
<< "Example 1: ./app_scout_demo can0" << std::endl;
return -1;
}
ScoutBase scout;
scout.Connect(device_name);
scout.EnableCommandedMode();
// // light control
std::cout << "Light: const off" << std::endl;
scout.SetLightCommand(CONST_OFF, 0, CONST_OFF, 0);
sleep(3);
std::cout << "Light: const on" << std::endl;
scout.SetLightCommand(CONST_ON, 0, CONST_ON, 0);
sleep(3);
std::cout << "Light: breath" << std::endl;
scout.SetLightCommand(BREATH, 0, BREATH, 0);
sleep(3);
std::cout << "Light: custom 90-80" << std::endl;
scout.SetLightCommand(CUSTOM, 90, CUSTOM, 80);
sleep(3);
std::cout << "Light: diabled cmd control" << std::endl;
scout.DisableLightControl();
int count = 0;
while (true) {
// motion control
if (count < 500) {
std::cout << "Motor: 0.2, 0" << std::endl;
scout.SetMotionCommand(0.2, 0.0);
}
auto state = scout.GetRobotState();
std::cout << "-------------------------------" << std::endl;
std::cout << "count: " << count << std::endl;
std::cout << "control mode: "
<< static_cast<int>(state.system_state.control_mode)
<< " , vehicle state: "
<< static_cast<int>(state.system_state.vehicle_state)
<< std::endl;
std::cout << "battery voltage: " << state.system_state.battery_voltage
<< std::endl;
std::cout << "velocity (linear, angular): "
<< state.motion_state.linear_velocity << ", "
<< state.motion_state.angular_velocity << std::endl;
std::cout << "-------------------------------" << std::endl;
usleep(20000);
++count;
}
return 0;
}

2
apps/tracer_demo/tracer_demo.cpp → demo/tracer_demo/tracer_demo.cpp
View File

@@ -7,7 +7,7 @@
* Copyright (c) 2019 Ruixiang Du (rdu) * Copyright (c) 2019 Ruixiang Du (rdu)
*/ */
#include "ugv_sdk/tracer_base.hpp" #include "ugv_sdk/protocol_v2/tracer_base.hpp"
using namespace westonrobot; using namespace westonrobot;

104
include/ugv_sdk/agilex_base.hpp
View File

@@ -1,104 +0,0 @@
/*
* agilex_base.hpp
*
* Created on: Dec 22, 2020 17:14
* Description:
*
* Each robot class derived from this base class should provide implementation
* for the following two functions:
*
* - virtual void Connect(std::string dev_name) = 0;
* - virtual void ParseCANFrame(can_frame *rx_frame) = 0;
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGILEX_BASE_HPP
#define AGILEX_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <atomic>
#include "ugv_sdk/details/async_port/async_can.hpp"
#include "ugv_sdk/agilex_message.h"
namespace westonrobot {
class AgilexBase {
public:
AgilexBase() = default;
virtual ~AgilexBase();
// do not allow copy or assignment
AgilexBase(const AgilexBase &hunter) = delete;
AgilexBase &operator=(const AgilexBase &hunter) = delete;
// any derived robot must implement this method with proper call back defined
virtual void Connect(std::string dev_name) = 0;
// cmd thread runs at 50Hz (20ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms);
// timeout: robot stops if user does not call SetMotionCommand() periodically
void EnableCmdTimeout(uint32_t timeout_ms = 100);
void DisableTimeout() { enable_timeout_ = false; }
// switch to commanded mode
void EnableCommandedMode();
// enforce 50Hz command loop for all AgileX robots internally
void SetMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity, double steering_angle);
// one-shot light command
void SendLightCommand(LightMode front_mode, uint8_t front_custom_value,
LightMode rear_mode, uint8_t rear_custom_value);
void DisableLightControl();
// motion mode change
void SetMotionMode(uint8_t mode);
// reset fault states
void ResetRobotState();
protected:
std::mutex state_mutex_;
std::mutex motion_cmd_mutex_;
MotionCommandMessage current_motion_cmd_;
// communication interface
bool can_connected_ = false;
std::shared_ptr<AsyncCAN> can_;
// timeout to be implemented in each vehicle
bool enable_timeout_ = true;
uint32_t timeout_ms_ = 500;
uint32_t watchdog_counter_ = 0;
void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; };
// command thread
std::thread cmd_thread_;
int32_t cmd_thread_period_ms_ = 20;
bool cmd_thread_started_ = false;
std::atomic<bool> keep_running_;
// internal functions
void StartCmdThread();
void ControlLoop(int32_t period_ms);
// connect to roboot from CAN or serial
using CANFrameRxCallback = AsyncCAN::ReceiveCallback;
void Connect(std::string dev_name, CANFrameRxCallback cb);
void Disconnect();
// ask background thread to shutdown properly
void Terminate();
void SendRobotCmd();
virtual void ParseCANFrame(can_frame *rx_frame) = 0;
};
} // namespace westonrobot
#endif /* AGILEX_BASE_HPP */

0
src/stopwatch.hpp → include/ugv_sdk/details/stopwatch.hpp
View File

0
include/ugv_sdk/agilex_message.h → include/ugv_sdk/interface/agilex_message.h
View File

50
include/ugv_sdk/interface/parser_interface.hpp Normal file
View File

@@ -0,0 +1,50 @@
/*
* parser_interface.hpp
*
* Created on: Jul 08, 2021 14:43
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#ifndef PASER_INTERFACE_HPP
#define PASER_INTERFACE_HPP
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#ifdef __linux__
#include <linux/can.h>
#else
struct can_frame {
uint32_t can_id;
uint8_t can_dlc;
uint8_t data[8] __attribute__((aligned(8)));
};
#endif
#include "ugv_sdk/interface/agilex_message.h"
struct ParserInterface {
// CAN support
virtual bool DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) = 0;
virtual void EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) = 0;
virtual uint8_t CalculateChecksum(uint16_t id, uint8_t *data,
uint8_t dlc) = 0;
// UART support
virtual bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg){
// throw exception
};
virtual void EncodeMessage(const AgxMessage *msg, uint8_t *buf, uint8_t *len){
// throw exception
};
virtual uint8_t CalculateChecksum(uint8_t *buf, uint8_t len){
// throw exception
};
};
#endif /* PASER_INTERFACE_HPP */

40
include/ugv_sdk/interface/robot_interface.hpp Normal file
View File

@@ -0,0 +1,40 @@
/*
* robot_interface.hpp
*
* Created on: Jul 08, 2021 11:48
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#ifndef ROBOT_INTERFACE_HPP
#define ROBOT_INTERFACE_HPP
#include <string>
#include "ugv_sdk/interface/agilex_message.h"
namespace westonrobot {
struct RobotInterface {
// functions to be implemented by class AgilexBase
virtual void EnableCommandedMode() = 0;
virtual void SetMotionMode(uint8_t mode){};
virtual void SendMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity,
double steering_angle) = 0;
virtual void SendLightCommand(LightMode front_mode,
uint8_t front_custom_value, LightMode rear_mode,
uint8_t rear_custom_value) = 0;
virtual void DisableLightControl() = 0;
// functions to be implemented by each robot class
virtual void Connect(std::string can_name) = 0;
virtual void Connect(std::string uart_name, uint32_t baudrate) = 0;
virtual void ResetRobotState() = 0;
};
} // namespace westonrobot
#endif /* ROBOT_INTERFACE_HPP */

38
include/ugv_sdk/interface/scout_interface.hpp
View File

@@ -1,8 +1,8 @@
/* /*
* scout_interface.hpp * scout_interface.hpp
* *
* Created on: Jul 07, 2021 09:10 * Created on: Jul 08, 2021 12:02
* Description: * Description: Scout-specific interface
* *
* Copyright (c) 2021 Weston Robot Pte. Ltd. * Copyright (c) 2021 Weston Robot Pte. Ltd.
*/ */
@@ -10,19 +10,33 @@
#ifndef SCOUT_INTERFACE_HPP #ifndef SCOUT_INTERFACE_HPP
#define SCOUT_INTERFACE_HPP #define SCOUT_INTERFACE_HPP
namespace westonrobot { #include <string>
class ScoutInterface {
public:
// set up connection
void Connect(std::string dev_name) override;
// robot control #include "ugv_sdk/interface/agilex_message.h"
void SetMotionCommand(double linear_vel, double angular_vel);
void SetLightCommand(const ScoutLightCmd &cmd); namespace westonrobot {
void DisableLightCmdControl(); struct ScoutState {
// system state
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
RcStateMessage rc_state;
ActuatorHSStateMessage actuator_hs_state[4];
ActuatorLSStateMessage actuator_ls_state[4];
// sensor data
OdometryMessage odometry;
};
struct ScoutInterface {
virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
virtual void SetLightCommand(LightMode f_mode, uint8_t f_value,
LightMode r_mode, uint8_t r_value) = 0;
// get robot state // get robot state
ScoutState GetScoutState(); virtual ScoutState GetRobotState() = 0;
}; };
} // namespace westonrobot } // namespace westonrobot

104
include/ugv_sdk/legacy/agilex_base_v1.hpp
View File

@@ -1,104 +0,0 @@
/*
* agilex_base.hpp
*
* Created on: Dec 22, 2020 17:14
* Description:
*
* Each robot class derived from this base class should provide implementation
* for the following two functions:
*
* - virtual void Connect(std::string dev_name) = 0;
* - virtual void ParseCANFrame(can_frame *rx_frame) = 0;
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGILEX_BASE_HPP
#define AGILEX_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <atomic>
#include "ugv_sdk/details/async_port/async_can.hpp"
#include "ugv_sdk/agilex_message.h"
namespace westonrobot {
class AgilexBase {
public:
AgilexBase() = default;
virtual ~AgilexBase();
// do not allow copy or assignment
AgilexBase(const AgilexBase &hunter) = delete;
AgilexBase &operator=(const AgilexBase &hunter) = delete;
// any derived robot must implement this method with proper call back defined
virtual void Connect(std::string dev_name) = 0;
// cmd thread runs at 50Hz (20ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms);
// timeout: robot stops if user does not call SetMotionCommand() periodically
void EnableCmdTimeout(uint32_t timeout_ms = 100);
void DisableTimeout() { enable_timeout_ = false; }
// switch to commanded mode
void EnableCommandedMode();
// enforce 50Hz command loop for all AgileX robots internally
void SetMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity, double steering_angle);
// one-shot light command
void SendLightCommand(LightMode front_mode, uint8_t front_custom_value,
LightMode rear_mode, uint8_t rear_custom_value);
void DisableLightControl();
// motion mode change
void SetMotionMode(uint8_t mode);
// reset fault states
void ResetRobotState();
protected:
std::mutex state_mutex_;
std::mutex motion_cmd_mutex_;
MotionCommandMessage current_motion_cmd_;
// communication interface
bool can_connected_ = false;
std::shared_ptr<AsyncCAN> can_;
// timeout to be implemented in each vehicle
bool enable_timeout_ = true;
uint32_t timeout_ms_ = 500;
uint32_t watchdog_counter_ = 0;
void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; };
// command thread
std::thread cmd_thread_;
int32_t cmd_thread_period_ms_ = 20;
bool cmd_thread_started_ = false;
std::atomic<bool> keep_running_;
// internal functions
void StartCmdThread();
void ControlLoop(int32_t period_ms);
// connect to roboot from CAN or serial
using CANFrameRxCallback = AsyncCAN::ReceiveCallback;
void Connect(std::string dev_name, CANFrameRxCallback cb);
void Disconnect();
// ask background thread to shutdown properly
void Terminate();
void SendRobotCmd();
virtual void ParseCANFrame(can_frame *rx_frame) = 0;
};
} // namespace westonrobot
#endif /* AGILEX_BASE_HPP */

151
include/ugv_sdk/mobile_base/agilex_base.hpp Normal file
View File

@@ -0,0 +1,151 @@
/*
* agilex_base.hpp
*
* Created on: Dec 22, 2020 17:14
* Description:
*
* Each robot class derived from this base class should provide implementation
* for the following two functions:
*
* - virtual void Connect(std::string dev_name) = 0;
* - virtual void ParseCANFrame(can_frame *rx_frame) = 0;
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGILEX_BASE_HPP
#define AGILEX_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <atomic>
#include "ugv_sdk/details/stopwatch.hpp"
#include "ugv_sdk/details/async_port/async_can.hpp"
#include "ugv_sdk/interface/robot_interface.hpp"
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot {
enum class AgilexProtocol { V1, V2 };
template <typename ParserType>
class AgilexBase : public RobotInterface {
public:
AgilexBase() = default;
virtual ~AgilexBase() { DisconnectPort(); }
// do not allow copy or assignment
AgilexBase(const AgilexBase &hunter) = delete;
AgilexBase &operator=(const AgilexBase &hunter) = delete;
// switch to commanded mode
void EnableCommandedMode() {
// construct message
AgxMessage msg;
msg.type = AgxMsgControlModeConfig;
msg.body.control_mode_config_msg.mode = CONTROL_MODE_CAN;
// encode msg to can frame and send to bus
can_frame frame;
parser_.EncodeMessage(&msg, &frame);
can_->SendFrame(frame);
}
// must be called at a frequency >= 50Hz
void SendMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity, double steering_angle) {
if (can_connected_) {
// motion control message
AgxMessage msg;
msg.type = AgxMsgMotionCommand;
msg.body.motion_command_msg.linear_velocity = linear_vel;
msg.body.motion_command_msg.angular_velocity = angular_vel;
msg.body.motion_command_msg.lateral_velocity = lateral_velocity;
msg.body.motion_command_msg.steering_angle = steering_angle;
// send to can bus
can_frame frame;
parser_.EncodeMessage(&msg, &frame);
can_->SendFrame(frame);
}
}
// one-shot light command
void SendLightCommand(LightMode front_mode, uint8_t front_custom_value,
LightMode rear_mode, uint8_t rear_custom_value) {
if (can_connected_) {
AgxMessage msg;
msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = true;
msg.body.light_command_msg.front_light.mode = front_mode;
msg.body.light_command_msg.front_light.custom_value = front_custom_value;
msg.body.light_command_msg.rear_light.mode = rear_mode;
msg.body.light_command_msg.rear_light.custom_value = rear_custom_value;
// send to can bus
can_frame frame;
parser_.EncodeMessage(&msg, &frame);
can_->SendFrame(frame);
}
}
void DisableLightControl() {
if (can_connected_) {
AgxMessage msg;
msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = false;
// send to can bus
can_frame frame;
parser_.EncodeMessage(&msg, &frame);
can_->SendFrame(frame);
}
}
// motion mode change
void SetMotionMode(uint8_t mode) {
if (can_connected_) {
AgxMessage msg;
msg.type = AgxMsgSetMotionMode;
msg.body.motion_mode_msg.motion_mode = mode;
// send to can bus
can_frame frame;
parser_.EncodeMessage(&msg, &frame);
can_->SendFrame(frame);
}
}
protected:
std::mutex state_mutex_;
// std::mutex motion_cmd_mutex_;
MotionCommandMessage current_motion_cmd_;
ParserType parser_;
// communication interface
bool can_connected_ = false;
std::shared_ptr<AsyncCAN> can_;
// connect to roboot from CAN or serial
using CANFrameRxCallback = AsyncCAN::ReceiveCallback;
void ConnectPort(std::string dev_name, CANFrameRxCallback cb) {
can_ = std::make_shared<AsyncCAN>(dev_name);
can_->SetReceiveCallback(cb);
can_->StartListening();
can_connected_ = true;
}
void DisconnectPort() {
if (can_connected_) can_->StopService();
}
virtual void ParseCANFrame(can_frame *rx_frame) = 0;
};
} // namespace westonrobot
#endif /* AGILEX_BASE_HPP */

55
include/ugv_sdk/ranger_base.hpp → include/ugv_sdk/mobile_base/ranger_interface.hpp
View File

@@ -1,21 +1,18 @@
/** /*
* @Kit : Qt-Creator: Desktop * ranger_interface.hpp
* @Author : Wang Zhe *
* @Date : 2021-04-19 19:12:52 * Created on: Jul 08, 2021 09:40
* @FileName : ranger_base.hpp * Description:
* @Mail : wangzheqie@qq.com *
* Copyright : AgileX Robotics * Copyright (c) 2021 Weston Robot Pte. Ltd.
**/ */
#ifndef RANGER_BASE_HPP #ifndef RANGER_INTERFACE_HPP
#define RANGER_BASE_HPP #define RANGER_INTERFACE_HPP
#include <cstdint>
#include <mutex>
#include <string> #include <string>
#include <thread>
#include "ugv_sdk/agilex_base.hpp" #include "ugv_sdk/interface/agilex_message.h"
namespace westonrobot { namespace westonrobot {
struct RangerState { struct RangerState {
@@ -24,8 +21,6 @@ struct RangerState {
MotionStateMessage motion_state; MotionStateMessage motion_state;
LightStateMessage light_state; LightStateMessage light_state;
RcStateMessage rc_state; RcStateMessage rc_state;
ActuatorHSStateMessage actuator_hs_state[8]; ActuatorHSStateMessage actuator_hs_state[8];
@@ -60,28 +55,18 @@ struct RangerLightCmd {
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
class RangerBase : public AgilexBase { struct RangerInterface {
public: virtual void Connect(std::string dev_name) = 0;
RangerBase() : AgilexBase(){};
~RangerBase() = default;
// set up connection
void Connect(std::string dev_name) override;
// robot control // robot control
void SetMotionCommand(double linear_vel, double steer_angle, virtual void SetMotionCommand(double linear_vel, double steer_angle,
double lateral_vel = 0.0, double angular_vel = 0.0); double lateral_vel, double angular_vel) = 0;
void SetLightCommand(const RangerLightCmd &cmd); virtual void SetLightCommand(const RangerLightCmd &cmd) = 0;
void SetMotionMode(uint8_t mode); virtual void SetMotionMode(uint8_t mode) = 0;
// get robot state // get robot state
RangerState GetRangerState(); virtual RangerState GetRangerState() = 0;
private:
RangerState ranger_state_;
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateRangerState(const AgxMessage &status_msg, RangerState &state);
}; };
} // namespace westonrobot } // namespace westonrobot
#endif // RANGER_BASE_HPP
#endif /* RANGER_INTERFACE_HPP */

27
include/ugv_sdk/mobile_base/scout_robot.hpp Normal file
View File

@@ -0,0 +1,27 @@
/*
* scout_robot.hpp
*
* Created on: Jul 08, 2021 10:59
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#ifndef SCOUT_ROBOT_HPP
#define SCOUT_ROBOT_HPP
#include <memory>
#include "ugv_sdk/mobile_base/common.hpp"
namespace westonrobot {
class ScoutRobot {
public:
ScoutRobot(AgilexProtocol protocol);
private:
std::unique_ptr<ScoutInterface> robot_;
};
} // namespace westonrobot
#endif /* SCOUT_ROBOT_HPP */

38
include/ugv_sdk/tracer_base.hpp → include/ugv_sdk/mobile_base/tracer_interface.hpp
View File

@@ -1,21 +1,18 @@
/* /*
* tracer_base.hpp * tracer_interface.hpp
* *
* Created on: Apr 14, 2020 10:21 * Created on: Jul 08, 2021 09:36
* Description: * Description:
* *
* Copyright (c) 2020 Weston Robot Pte. Ltd. * Copyright (c) 2021 Weston Robot Pte. Ltd.
*/ */
#ifndef TRACER_BASE_HPP #ifndef TRACER_INTERFACE_HPP
#define TRACER_BASE_HPP #define TRACER_INTERFACE_HPP
#include <string> #include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include "ugv_sdk/agilex_base.hpp" #include "ugv_sdk/interface/agilex_message.h"
namespace westonrobot { namespace westonrobot {
struct TracerState { struct TracerState {
@@ -52,27 +49,18 @@ struct TracerLightCmd {
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
class TracerBase : public AgilexBase { struct TracerInterface {
public:
TracerBase() : AgilexBase(){};
~TracerBase() = default;
// set up connection // set up connection
void Connect(std::string dev_name) override; virtual void Connect(std::string can_name) = 0;
virtual void Connect(std::string uart_name, uint32_t baudrate) = 0;
// robot control // robot control
void SetMotionCommand(double linear_vel, double angular_vel); virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
void SetLightCommand(const TracerLightCmd &cmd); virtual void SetLightCommand(const TracerLightCmd &cmd) = 0;
// get robot state // get robot state
TracerState GetTracerState(); virtual TracerState GetTracerState() = 0;
private:
TracerState tracer_state_;
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateTracerState(const AgxMessage &status_msg, TracerState &state);
}; };
} // namespace westonrobot } // namespace westonrobot
#endif /* TRACER_BASE_HPP */ #endif /* TRACER_INTERFACE_HPP */

2
include/ugv_sdk/legacy/scout_can_parser_v1.h → include/ugv_sdk/protocol_v1/scout_can_parser_v1.h
View File

@@ -17,7 +17,7 @@ extern "C" {
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include "ugv_sdk/legacy/scout_protocol_v1.h" #include "ugv_sdk/protocol_v1/scout_protocol_v1.h"
#ifdef __linux__ #ifdef __linux__
#include <linux/can.h> #include <linux/can.h>

0
include/ugv_sdk/legacy/scout_protocol_v1.h → include/ugv_sdk/protocol_v1/scout_protocol_v1.h
View File

2
include/ugv_sdk/details/agilex_msg_parser.h → include/ugv_sdk/protocol_v2/agilex_msg_parser.h
View File

@@ -28,7 +28,7 @@ struct can_frame {
}; };
#endif #endif
#include "ugv_sdk/agilex_message.h" #include "ugv_sdk/interface/agilex_message.h"
bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg); bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg);
void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame); void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame);

26
include/ugv_sdk/protocol_v2/protocol_v2_parser.hpp Normal file
View File

@@ -0,0 +1,26 @@
/*
* protocol_v2_parser.hpp
*
* Created on: Jul 08, 2021 14:51
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#ifndef PROTOCOL_V2_PARSER_HPP
#define PROTOCOL_V2_PARSER_HPP
#include "ugv_sdk/interface/parser_interface.hpp"
namespace westonrobot {
class ProtocolV2Parser : public ParserInterface {
public:
bool DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) override;
void EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) override;
uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override;
};
} // namespace westonrobot
#endif /* PROTOCOL_V2_PARSER_HPP */

46
include/ugv_sdk/protocol_v2/ranger_base.hpp Normal file
View File

@@ -0,0 +1,46 @@
/**
* @Kit : Qt-Creator: Desktop
* @Author : Wang Zhe
* @Date : 2021-04-19 19:12:52
* @FileName : ranger_base.hpp
* @Mail : wangzheqie@qq.com
* Copyright : AgileX Robotics
**/
#ifndef RANGER_BASE_HPP
#define RANGER_BASE_HPP
#include <cstdint>
#include <mutex>
#include <string>
#include <thread>
#include "ugv_sdk/interface/ranger_interface.hpp"
#include "ugv_sdk/protocol_v2/agilex_base.hpp"
namespace westonrobot {
class RangerBase : public AgilexBase, public RangerInterface {
public:
RangerBase() : AgilexBase(){};
~RangerBase() = default;
// set up connection
void Connect(std::string dev_name) override;
// robot control
void SetMotionCommand(double linear_vel, double steer_angle,
double lateral_vel = 0.0, double angular_vel = 0.0);
void SetLightCommand(const RangerLightCmd &cmd);
void SetMotionMode(uint8_t mode);
// get robot state
RangerState GetRangerState();
private:
RangerState ranger_state_;
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateRangerState(const AgxMessage &status_msg, RangerState &state);
};
} // namespace westonrobot
#endif // RANGER_BASE_HPP

51
include/ugv_sdk/protocol_v2/scout_base_v2.hpp Normal file
View File

@@ -0,0 +1,51 @@
/*
* scout_base.hpp
*
* Created on: Dec 23, 2020 14:39
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef SCOUT_BASE_HPP
#define SCOUT_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include "ugv_sdk/interface/scout_interface.hpp"
#include "ugv_sdk/mobile_base/agilex_base.hpp"
#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp"
namespace westonrobot {
class ScoutBase : public AgilexBase<ProtocolV2Parser>, public ScoutInterface {
public:
ScoutBase() : AgilexBase(){};
~ScoutBase() = default;
// set up connection
void Connect(std::string can_name) override;
void Connect(std::string uart_name, uint32_t baudrate) override;
// robot control
void SetMotionCommand(double linear_vel, double angular_vel) override;
void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value) override;
// get robot state
ScoutState GetRobotState() override;
void ResetRobotState() override {}
private:
ScoutState scout_state_;
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state);
};
} // namespace westonrobot
#endif /* SCOUT_BASE_HPP */

46
include/ugv_sdk/protocol_v2/tracer_base.hpp Normal file
View File

@@ -0,0 +1,46 @@
/*
* tracer_base.hpp
*
* Created on: Apr 14, 2020 10:21
* Description:
*
* Copyright (c) 2020 Weston Robot Pte. Ltd.
*/
#ifndef TRACER_BASE_HPP
#define TRACER_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include "ugv_sdk/interface/tracer_interface.hpp"
#include "ugv_sdk/protocol_v2/agilex_base.hpp"
namespace westonrobot {
class TracerBase : public AgilexBase, public TracerInterface {
public:
TracerBase() : AgilexBase(){};
~TracerBase() = default;
// set up connection
void Connect(std::string can_name) override;
void Connect(std::string uart_name, uint32_t baudrate) override;
// robot control
void SetMotionCommand(double linear_vel, double angular_vel);
void SetLightCommand(const TracerLightCmd &cmd);
// get robot state
TracerState GetTracerState();
private:
TracerState tracer_state_;
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateTracerState(const AgxMessage &status_msg, TracerState &state);
};
} // namespace westonrobot
#endif /* TRACER_BASE_HPP */

83
include/ugv_sdk/scout_base.hpp
View File

@@ -1,83 +0,0 @@
/*
* scout_base.hpp
*
* Created on: Dec 23, 2020 14:39
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef SCOUT_BASE_HPP
#define SCOUT_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include "ugv_sdk/agilex_base.hpp"
namespace westonrobot {
struct ScoutState {
// system state
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
RcStateMessage rc_state;
ActuatorHSStateMessage actuator_hs_state[4];
ActuatorLSStateMessage actuator_ls_state[4];
// sensor data
OdometryMessage odometry;
};
struct ScoutMotionCmd {
double linear_velocity;
double angular_velocity;
};
struct ScoutLightCmd {
ScoutLightCmd() = default;
ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value)
: cmd_ctrl_allowed(true),
front_mode(f_mode),
front_custom_value(f_value),
rear_mode(r_mode),
rear_custom_value(r_value) {}
bool cmd_ctrl_allowed = false;
LightMode front_mode;
uint8_t front_custom_value;
LightMode rear_mode;
uint8_t rear_custom_value;
};
/////////////////////////////////////////////////////////////////////////
class ScoutBase : public AgilexBase {
public:
ScoutBase() : AgilexBase(){};
~ScoutBase() = default;
// set up connection
void Connect(std::string dev_name) override;
// robot control
void SetMotionCommand(double linear_vel, double angular_vel);
void SetLightCommand(const ScoutLightCmd &cmd);
// get robot state
ScoutState GetScoutState();
private:
ScoutState scout_state_;
void ParseCANFrame(can_frame *rx_frame) override;
void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state);
};
} // namespace westonrobot
#endif /* SCOUT_BASE_HPP */

170
src/agilex_base.cpp
View File

@@ -1,170 +0,0 @@
/*
* agilex_base.cpp
*
* Created on: Dec 22, 2020 17:20
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "ugv_sdk/agilex_base.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h"
#include "stopwatch.hpp"
namespace westonrobot {
AgilexBase::~AgilexBase() {
// release resource if occupied
Disconnect();
// joint cmd thread
if (cmd_thread_.joinable()) cmd_thread_.join();
}
void AgilexBase::Connect(std::string dev_name, CANFrameRxCallback cb) {
can_ = std::make_shared<AsyncCAN>(dev_name);
can_->SetReceiveCallback(cb);
can_->StartListening();
can_connected_ = true;
}
void AgilexBase::Disconnect() {
if (can_connected_) can_->StopService();
}
void AgilexBase::Terminate() {
keep_running_ = false;
std::terminate();
}
void AgilexBase::SetCmdThreadPeriodMs(int32_t period_ms) {
cmd_thread_period_ms_ = period_ms;
};
void AgilexBase::EnableCmdTimeout(uint32_t timeout_ms) {
enable_timeout_ = true;
timeout_ms_ = timeout_ms;
}
void AgilexBase::StartCmdThread() {
keep_running_ = true;
cmd_thread_ = std::thread(
std::bind(&AgilexBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void AgilexBase::ControlLoop(int32_t period_ms) {
uint32_t timeout_iter_num;
if (enable_timeout_) {
if (timeout_ms_ < period_ms) timeout_ms_ = period_ms;
timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms);
// std::cout << "Timeout iteration number: " << timeout_iter_num <<
// std::endl;
}
Timer tm;
while (keep_running_) {
tm.reset();
if (enable_timeout_) {
if (watchdog_counter_ < timeout_iter_num) {
SendRobotCmd();
++watchdog_counter_;
}
// else {
// std::cout << "Warning: cmd timeout, no cmd sent to robot" <<
// std::endl;
// }
} else {
SendRobotCmd();
}
tm.sleep_until_ms(period_ms);
}
}
void AgilexBase::EnableCommandedMode() {
// construct message
AgxMessage msg;
msg.type = AgxMsgControlModeConfig;
msg.body.control_mode_config_msg.mode = CONTROL_MODE_CAN;
// encode msg to can frame and send to bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
void AgilexBase::SetMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity,
double steering_angle) {
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread();
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = linear_vel;
current_motion_cmd_.angular_velocity = angular_vel;
current_motion_cmd_.lateral_velocity = lateral_velocity;
current_motion_cmd_.steering_angle = steering_angle;
FeedCmdTimeoutWatchdog();
}
void AgilexBase::SendRobotCmd() {
if (can_connected_) {
// motion control message
AgxMessage msg;
msg.type = AgxMsgMotionCommand;
motion_cmd_mutex_.lock();
msg.body.motion_command_msg = current_motion_cmd_;
motion_cmd_mutex_.unlock();
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
}
void AgilexBase::SendLightCommand(LightMode front_mode,
uint8_t front_custom_value,
LightMode rear_mode,
uint8_t rear_custom_value) {
AgxMessage msg;
msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = true;
msg.body.light_command_msg.front_light.mode = front_mode;
msg.body.light_command_msg.front_light.custom_value = front_custom_value;
msg.body.light_command_msg.rear_light.mode = rear_mode;
msg.body.light_command_msg.rear_light.custom_value = rear_custom_value;
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
void AgilexBase::DisableLightControl() {
AgxMessage msg;
msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = false;
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
void AgilexBase::SetMotionMode(uint8_t mode) {
AgxMessage msg;
msg.type = AgxMsgSetMotionMode;
msg.body.motion_mode_msg.motion_mode = mode;
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
} // namespace westonrobot

14
src/mobile_base/scout_robot.cpp Normal file
View File

@@ -0,0 +1,14 @@
/*
* scout_robot.cpp
*
* Created on: Jul 08, 2021 11:13
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include "ugv_sdk/mobile_base/scout_robot.hpp"
namespace westonrobot {
ScoutRobot::ScoutRobot(AgilexProtocol protocol) {}
} // namespace westonrobot

2
src/legacy/scout_can_parser_v1.c → src/protocol_v1/scout_can_parser_v1.c
View File

@@ -7,7 +7,7 @@
* Copyright (c) 2019 Ruixiang Du (rdu) * Copyright (c) 2019 Ruixiang Du (rdu)
*/ */
#include "ugv_sdk/legacy/scout_can_parser_v1.h" #include "ugv_sdk/protocol_v1/scout_can_parser_v1.h"
#include "string.h" #include "string.h"

2
src/agx_msg_parser.c → src/protocol_v2/agx_msg_parser.c
View File

@@ -8,7 +8,7 @@
*/ */
#include "agx_protocol_v2.h" #include "agx_protocol_v2.h"
#include "ugv_sdk/details/agilex_msg_parser.h" #include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
#include "stdio.h" #include "stdio.h"
#include "string.h" #include "string.h"

0
src/agx_protocol_v2.h → src/protocol_v2/agx_protocol_v2.h
View File

29
src/protocol_v2/protocol_v2_parser.cpp Normal file
View File

@@ -0,0 +1,29 @@
/*
* protocol_v2_parser.cpp
*
* Created on: Jul 08, 2021 14:53
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp"
namespace westonrobot {
bool ProtocolV2Parser::DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) {
return DecodeCanFrame(rx_frame, msg);
}
void ProtocolV2Parser::EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) {
EncodeCanFrame(msg, tx_frame);
}
uint8_t ProtocolV2Parser::CalculateChecksum(uint16_t id, uint8_t *data,
uint8_t dlc) {
return CalcCanFrameChecksum(id, data, dlc);
}
} // namespace westonrobot

6
src/ranger_base.cpp → src/protocol_v2/ranger_base.cpp
View File

@@ -7,7 +7,7 @@
* Copyright : AgileX Robotics * Copyright : AgileX Robotics
**/ **/
#include "ugv_sdk/ranger_base.hpp" #include "ugv_sdk/protocol_v2/ranger_base.hpp"
#include <algorithm> #include <algorithm>
#include <array> #include <array>
@@ -21,11 +21,11 @@
#include "stopwatch.hpp" #include "stopwatch.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h" #include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot { namespace westonrobot {
void RangerBase::Connect(std::string dev_name) { void RangerBase::Connect(std::string dev_name) {
AgilexBase::Connect(dev_name, std::bind(&RangerBase::ParseCANFrame, this, AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame, this,
std::placeholders::_1)); std::placeholders::_1));
} }

35
src/scout_base.cpp → src/protocol_v2/scout_base.cpp
View File

@@ -1,4 +1,13 @@
#include "ugv_sdk/scout_base.hpp" /*
* scout_base.cpp
*
* Created on: Jul 08, 2021 12:07
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include "ugv_sdk/protocol_v2/scout_base_v2.hpp"
#include <string> #include <string>
#include <cstring> #include <cstring>
@@ -10,28 +19,28 @@
#include <ratio> #include <ratio>
#include <thread> #include <thread>
#include "stopwatch.hpp" #include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
#include "ugv_sdk/details/agilex_msg_parser.h"
namespace westonrobot { namespace westonrobot {
void ScoutBase::Connect(std::string dev_name) { void ScoutBase::Connect(std::string dev_name) {
AgilexBase::Connect(dev_name, std::bind(&ScoutBase::ParseCANFrame, this, AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBase::ParseCANFrame, this,
std::placeholders::_1)); std::placeholders::_1));
}
void ScoutBase::Connect(std::string uart_name, uint32_t baudrate) {
// TODO
} }
void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) { void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) {
AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0); AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
} }
void ScoutBase::SetLightCommand(const ScoutLightCmd &cmd) { void ScoutBase::SetLightCommand(LightMode f_mode, uint8_t f_value,
if (cmd.cmd_ctrl_allowed) { LightMode r_mode, uint8_t r_value) {
AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value, AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value);
LightMode::CONST_OFF, 0);
}
} }
ScoutState ScoutBase::GetScoutState() { ScoutState ScoutBase::GetRobotState() {
std::lock_guard<std::mutex> guard(state_mutex_); std::lock_guard<std::mutex> guard(state_mutex_);
return scout_state_; return scout_state_;
} }

12
src/tracer_base.cpp → src/protocol_v2/tracer_base.cpp
View File

@@ -1,4 +1,4 @@
#include "ugv_sdk/tracer_base.hpp" #include "ugv_sdk/protocol_v2/tracer_base.hpp"
#include <string> #include <string>
#include <cstring> #include <cstring>
@@ -12,12 +12,16 @@
#include "stopwatch.hpp" #include "stopwatch.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h" #include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot { namespace westonrobot {
void TracerBase::Connect(std::string dev_name) { void TracerBase::Connect(std::string dev_name) {
AgilexBase::Connect(dev_name, std::bind(&TracerBase::ParseCANFrame, this, AgilexBase::ConnectPort(dev_name, std::bind(&TracerBase::ParseCANFrame, this,
std::placeholders::_1)); std::placeholders::_1));
}
void TracerBase::Connect(std::string uart_name, uint32_t baudrate) {
// TODO
} }
void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) { void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) {