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

moved ugv_sdk out from sub-folder

Browse Source
This commit is contained in:
Ruixiang Du
2021-03-31 12:14:40 +08:00
parent 0316afa8a6
commit ed2f38a7fd
42 changed files with 406 additions and 267 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
apps/CMakeLists.txt Executable file
View File

@@ -0,0 +1,19 @@
# 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)
# if(BUILD_MONITOR)
# add_subdirectory(scout_monitor)
# endif()

92
apps/hunter_demo/hunter_demo.cpp Normal file
View File

@@ -0,0 +1,92 @@
/*
* demo_hunter_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/hunter/hunter_base.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
{
std::cout << "Usage: app_hunter_demo <interface>" << std::endl
<< "Example 1: ./app_hunter_demo can0" << std::endl;
return -1;
}
HunterBase hunter;
hunter.Connect(device_name);
int count = 0;
while (true)
{
// motion control
if (count < 5)
{
std::cout << "Motor: 0.2, 0.0" << std::endl;
hunter.SetMotionCommand(0.2, 0.0);
}
else if (count < 10)
{
std::cout << "Motor: 0.8, 0.3" << std::endl;
hunter.SetMotionCommand(0.8, 0.3);
}
else if (count < 15)
{
std::cout << "Motor: 1.5, 0.5" << std::endl;
hunter.SetMotionCommand(1.5, 0.5);
}
else if (count < 20)
{
std::cout << "Motor: 1.0, 0.3" << std::endl;
hunter.SetMotionCommand(1.0, 0.3);
}
else if (count < 25)
{
std::cout << "Motor: 0.0, 0.0" << std::endl;
hunter.SetMotionCommand(0.0, 0.0);
}
else if (count < 30)
{
std::cout << "Motor: -0.5, -0.3" << std::endl;
hunter.SetMotionCommand(-0.5, -0.3);
}
else if (count < 35)
{
std::cout << "Motor: -1.0, -0.5" << std::endl;
hunter.SetMotionCommand(-1.0, -0.5);
}
else if (count < 40)
{
std::cout << "Motor: 0.0, 0.0," << std::endl;
hunter.SetMotionCommand(0.0, 0.0);
}
auto state = hunter.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;
}

122
apps/scout_demo/scout_demo.cpp Normal file
View File

@@ -0,0 +1,122 @@
/*
* demo_scout_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/scout/scout_base.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_demo <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, baud_rate);
// // light control
// std::cout << "Light: const off" << std::endl;
// scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_OFF, 0,
// ScoutLightCmd::LightMode::CONST_OFF, 0});
// sleep(3);
// std::cout << "Light: const on" << std::endl;
// scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0,
// ScoutLightCmd::LightMode::CONST_ON, 0});
// sleep(3);
// std::cout << "Light: breath" << std::endl;
// scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0,
// ScoutLightCmd::LightMode::BREATH, 0});
// sleep(3);
// std::cout << "Light: custom 90-80" << std::endl;
// scout.SetLightCommand({ScoutLightCmd::LightMode::CUSTOM, 90,
// ScoutLightCmd::LightMode::CUSTOM, 80});
// sleep(3);
// std::cout << "Light: diabled cmd control" << std::endl;
// scout.DisableLightCmdControl();
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({ScoutLightCmd::LightMode::BREATH, 0,
// ScoutLightCmd::LightMode::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.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;
usleep(20000);
++count;
}
return 0;
}

74
apps/scout_demo/scout_discharge.cpp Normal file
View File

@@ -0,0 +1,74 @@
/*
* demo_scout_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "scout_base/scout_base.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_demo can0" << std::endl
<< "Example 2: ./app_scout_demo /dev/ttyUSB0 115200" << std::endl;
return -1;
}
ScoutBase scout;
scout.Connect(device_name, baud_rate);
// light control
std::cout << "Light: const on" << std::endl;
scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0});
int count = 0;
while (true)
{
auto state = scout.GetScoutState();
if (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.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;
}
else
{
scout.SetMotionCommand(0, 0);
std::cout << "-------------------------------" << std::endl;
std::cout << "discharge stopped at: " << state.battery_voltage << " V" << std::endl;
std::cout << "-------------------------------" << std::endl;
}
sleep(1);
++count;
}
return 0;
}

24
apps/scout_monitor/CMakeLists.txt Normal file
View File

@@ -0,0 +1,24 @@
# Dependency libraries
set(CURSES_NEED_NCURSES TRUE)
find_package(Curses REQUIRED)
# Add libraries
set(SCOUT_MONITOR_SRC
src/nshapes.cpp
src/ncolors.cpp
src/scout_monitor.cpp
)
add_library(monitor STATIC ${SCOUT_MONITOR_SRC})
target_link_libraries(monitor 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 Normal file
View File

@@ -0,0 +1,3 @@
# 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 Normal file
View File

@@ -0,0 +1,40 @@
#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 Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 85 KiB

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

@@ -0,0 +1,67 @@
/*
* ncolors.hpp
*
* Created on: Jun 20, 2019 06:22
* Description:
*
* Original source: https://www.linuxjournal.com/content/about-ncurses-colors-0
* This copy is based on the original implementation, modified and
* maintained by Ruixiang.
*
* Copyright (c) 2018 Jim Hall
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef NCOLORS_HPP
#define NCOLORS_HPP
#include <ncurses.h>
namespace 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 Normal file
View File

@@ -0,0 +1,24 @@
/*
* nshapes.hpp
*
* Created on: Jun 20, 2019 06:21
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef NSHAPES_HPP
#define NSHAPES_HPP
#include <ncurses.h>
namespace 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 Normal file
View File

@@ -0,0 +1,78 @@
/*
* 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/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 Normal file
View File

@@ -0,0 +1,116 @@
/*
* 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 Normal file
View File

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

@@ -0,0 +1,658 @@
/*
* 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 Normal file
View File

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

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

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

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

@@ -0,0 +1,65 @@
#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 Normal file
View File

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

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

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

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

100
apps/tracer_demo/tracer_demo.cpp Normal file
View File

@@ -0,0 +1,100 @@
/*
* demo_tracer_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/tracer/tracer_base.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 {
std::cout << "Usage: app_tracer_demo <interface>" << std::endl
<< "Example 1: ./app_tracer_demo can0" << std::endl;
return -1;
}
TracerBase tracer;
tracer.Connect(device_name);
tracer.EnableCommandedMode();
// tracer.DisableTimeout();
// light control
std::cout << "Light: const off" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0});
// usleep(50000);
sleep(3);
std::cout << "Light: const on" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0});
// usleep(50000);
sleep(3);
std::cout << "Light: breath" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0});
// usleep(50000);
sleep(8);
std::cout << "Light: custom 90-80" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90});
// usleep(50000);
sleep(3);
std::cout << "Light: diabled cmd control" << std::endl;
tracer.SetLightCommand(TracerLightCmd());
int count = 0;
while (true) {
// motion control
if (count < 5) {
std::cout << "Motor: 0.2, 0.0" << std::endl;
tracer.SetMotionCommand(0.2, 0.0);
} else if (count < 10) {
std::cout << "Motor: 0.8, 0.3" << std::endl;
tracer.SetMotionCommand(0.8, 0.3);
} else if (count < 15) {
std::cout << "Motor: 1.5, 0.5" << std::endl;
tracer.SetMotionCommand(1.5, 0.5);
} else if (count < 20) {
std::cout << "Motor: 1.0, 0.3" << std::endl;
tracer.SetMotionCommand(1.0, 0.3);
} else if (count < 25) {
std::cout << "Motor: 0.0, 0.0" << std::endl;
tracer.SetMotionCommand(0.0, 0.0);
} else if (count < 30) {
std::cout << "Motor: -0.5, -0.3" << std::endl;
tracer.SetMotionCommand(-0.5, -0.3);
} else if (count < 35) {
std::cout << "Motor: -1.0, -0.5" << std::endl;
tracer.SetMotionCommand(-1.0, -0.5);
} else if (count < 40) {
std::cout << "Motor: 0.0, 0.0," << std::endl;
tracer.SetMotionCommand(0.0, 0.0);
}
// tracer.SetMotionCommand(0.8, 0.8);
auto state = tracer.GetTracerState();
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.angular_velocity << std::endl;
std::cout << "-------------------------------" << std::endl;
// usleep(20000);
sleep(1);
++count;
}
return 0;
}