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

added tracer sdk

Browse Source
This commit is contained in:
Ruixiang Du
2020-04-14 12:20:56 +08:00
parent 97d63628a5
commit cc8af01bec
34 changed files with 1194 additions and 2585 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/CMakeLists.txt
View File

@@ -1,5 +1,6 @@
# Add source directories
add_subdirectory(apps)
add_subdirectory(common)
add_subdirectory(hunter_sdk)
add_subdirectory(scout_sdk)
add_subdirectory(hunter_sdk)
add_subdirectory(tracer_sdk)

3
src/apps/CMakeLists.txt
View File

@@ -11,6 +11,9 @@ target_link_libraries(app_scout_demo scoutbase)
add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp)
target_link_libraries(app_hunter_demo hunterbase)
add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp)
target_link_libraries(app_tracer_demo tracerbase)
if(BUILD_MONITOR)
add_subdirectory(scout_monitor)
endif()

108
src/apps/tracer_demo/tracer_demo.cpp Normal file
View File

@@ -0,0 +1,108 @@
/*
* demo_tracer_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "tracer_base/tracer_base.hpp"
using namespace wescore;
int main(int argc, char **argv)
{
std::string device_name;
int32_t baud_rate = 0;
if (argc == 2)
{
device_name = {argv[1]};
std::cout << "Specified CAN: " << device_name << std::endl;
}
else
{
std::cout << "Usage: app_tracer_demo <interface>" << std::endl
<< "Example 1: ./app_tracer_demo can0" << std::endl;
return -1;
}
TracerBase tracer;
tracer.Connect(device_name);
// light control
std::cout << "Light: const off" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0, TracerLightCmd::LightMode::CONST_OFF, 0});
sleep(3);
std::cout << "Light: const on" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0, TracerLightCmd::LightMode::CONST_ON, 0});
sleep(3);
std::cout << "Light: breath" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0, TracerLightCmd::LightMode::BREATH, 0});
sleep(3);
std::cout << "Light: custom 90-80" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90, TracerLightCmd::LightMode::CUSTOM, 80});
sleep(3);
std::cout << "Light: diabled cmd control" << std::endl;
tracer.DisableLightCmdControl();
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);
}
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;
sleep(1);
++count;
}
return 0;
}

8
src/hunter_sdk/hunter_base/CMakeLists.txt
View File

@@ -9,7 +9,7 @@ target_include_directories(hunterbase PUBLIC
$<INSTALL_INTERFACE:include>
PRIVATE src)
## Add executables
# if(BUILD_TESTS)
# add_subdirectory(tests)
# endif()
## Add executables
if(BUILD_TESTS)
add_subdirectory(tests)
endif()

14
src/hunter_sdk/hunter_base/tests/CMakeLists.txt
View File

@@ -2,15 +2,5 @@
#find_package(LIBRARY_NAME REQUIRED)
# tests
add_executable(test_scout_base test_scout_base.cpp)
target_link_libraries(test_scout_base scoutbase)
# add_executable(test_scout_serial test_scout_serial.cpp)
# target_link_libraries(test_scout_serial scoutbase)
# add_executable(test_can_msg test_can_msg.cpp)
# target_link_libraries(test_can_msg scoutbase)
# add_executable(test_serial_parser test_serial_parser.cpp)
# target_link_libraries(test_serial_parser scoutbase)
add_executable(test_hunter_base test_hunter_base.cpp)
target_link_libraries(test_hunter_base hunterbase)

39
src/hunter_sdk/hunter_base/tests/test_can_msg.cpp
View File

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

54
src/hunter_sdk/hunter_base/tests/test_hunter_base.cpp Normal file
View File

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

45
src/hunter_sdk/hunter_base/tests/test_scout_serial.cpp
View File

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

51
src/hunter_sdk/hunter_base/tests/test_serial_parser.cpp
View File

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

3
src/tracer_sdk/CMakeLists.txt Executable file
View File

@@ -0,0 +1,3 @@
# Add source directories
add_subdirectory(tracer_base)
add_subdirectory(tracer_protocol)

16
src/tracer_sdk/tracer_base/CMakeLists.txt Executable file
View File

@@ -0,0 +1,16 @@
## Add libraries
set(TRACER_BASE_SRC
src/tracer_base.cpp
)
add_library(tracerbase STATIC ${TRACER_BASE_SRC})
target_link_libraries(tracerbase asyncio tracer_protocol stopwatch)
target_include_directories(tracerbase PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<BUILD_INTERFACE:${Boost_INCLUDE_DIRS}>
$<INSTALL_INTERFACE:include>
PRIVATE src)
## Add executables
if(BUILD_TESTS)
add_subdirectory(tests)
endif()

109
src/tracer_sdk/tracer_base/include/tracer_base/tracer_base.hpp Normal file
View File

@@ -0,0 +1,109 @@
/*
* tracer_base.hpp
*
* Created on: Apr 14, 2020 10:21
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_BASE_HPP
#define TRACER_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#include "async_io/async_can.hpp"
#include "async_io/async_serial.hpp"
#include "tracer_protocol/tracer_protocol.h"
#include "tracer_protocol/tracer_can_parser.h"
#include "tracer_base/tracer_types.hpp"
namespace wescore
{
class TracerBase
{
public:
TracerBase() = default;
~TracerBase();
// do not allow copy
TracerBase(const TracerBase &tracer) = delete;
TracerBase &operator=(const TracerBase &tracer) = delete;
public:
// connect to roboot from CAN
void Connect(std::string dev_name);
// disconnect from roboot, only valid for serial port
void Disconnect();
// cmd thread runs at 100Hz (10ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; };
// motion control
void SetMotionCommand(double linear_vel, double angular_vel,
TracerMotionCmd::FaultClearFlag fault_clr_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT);
// light control
void SetLightCommand(TracerLightCmd cmd);
void DisableLightCmdControl();
// get robot state
TracerState GetTracerState();
private:
// hardware communication interface
std::shared_ptr<ASyncCAN> can_if_;
std::shared_ptr<ASyncSerial> serial_if_;
// CAN priority higher than serial if both connected
bool can_connected_ = false;
bool serial_connected_ = false;
// serial port related variables
uint8_t tx_cmd_len_;
uint8_t tx_buffer_[TRACER_CMD_BUF_LEN];
// cmd/status update related variables
std::thread cmd_thread_;
std::mutex tracer_state_mutex_;
std::mutex motion_cmd_mutex_;
std::mutex light_cmd_mutex_;
TracerState tracer_state_;
TracerMotionCmd current_motion_cmd_;
TracerLightCmd current_light_cmd_;
int32_t cmd_thread_period_ms_ = 10;
bool cmd_thread_started_ = false;
bool light_ctrl_enabled_ = false;
bool light_ctrl_requested_ = false;
// internal functions
void ConfigureCANBus(const std::string &can_if_name = "can1");
void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", int32_t baud_rate = 115200);
void StartCmdThread();
void ControlLoop(int32_t period_ms);
void SendMotionCmd(uint8_t count);
void SendLightCmd(uint8_t count);
void ParseCANFrame(can_frame *rx_frame);
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received);
void NewStatusMsgReceivedCallback(const TracerMessage &msg);
public:
static void UpdateTracerState(const TracerMessage &status_msg, TracerState &state);
};
} // namespace wescore
#endif /* TRACER_BASE_HPP */

111
src/tracer_sdk/tracer_base/include/tracer_base/tracer_types.hpp Normal file
View File

@@ -0,0 +1,111 @@
/*
* tracer_types.hpp
*
* Created on: Apr 14, 2020 10:22
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_TYPES_HPP
#define TRACER_TYPES_HPP
#include <cstdint>
#include <iostream>
namespace wescore
{
struct TracerState
{
enum MotorID
{
FRONT_RIGHT = 0,
FRONT_LEFT = 1,
REAR_LEFT = 2,
REAR_RIGHT = 3
};
struct MotorState
{
double current = 0; // in A
double rpm = 0;
double temperature = 0;
};
struct LightState
{
uint8_t mode = 0;
uint8_t custom_value = 0;
};
// base state
uint8_t base_state = 0;
uint8_t control_mode = 0;
uint16_t fault_code = 0;
double battery_voltage = 0.0;
// motor state
MotorState motor_states[4];
// light state
bool light_control_enabled = false;
LightState front_light_state;
LightState rear_light_state;
// motion state
double linear_velocity = 0;
double angular_velocity = 0;
};
struct TracerMotionCmd
{
enum class FaultClearFlag
{
NO_FAULT = 0x00,
BAT_UNDER_VOL = 0x01,
BAT_OVER_VOL = 0x02,
MOTOR1_COMM = 0x03,
MOTOR2_COMM = 0x04,
MOTOR3_COMM = 0x05,
MOTOR4_COMM = 0x06,
MOTOR_DRV_OVERHEAT = 0x07,
MOTOR_OVERCURRENT = 0x08
};
TracerMotionCmd(int8_t linear = 0, int8_t angular = 0,
FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT)
: linear_velocity(linear), angular_velocity(angular),
fault_clear_flag(fault_clr_flag) {}
int8_t linear_velocity;
int8_t angular_velocity;
FaultClearFlag fault_clear_flag;
static constexpr double max_linear_velocity = 1.5; // 1.5 m/s
static constexpr double min_linear_velocity = -1.5; // -1.5 m/s
static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s
static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s
};
struct TracerLightCmd
{
enum class LightMode
{
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
};
TracerLightCmd() = default;
TracerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value),
rear_mode(r_mode), rear_custom_value(r_value) {}
LightMode front_mode;
uint8_t front_custom_value;
LightMode rear_mode;
uint8_t rear_custom_value;
};
} // namespace wescore
#endif /* TRACER_TYPES_HPP */

338
src/tracer_sdk/tracer_base/src/tracer_base.cpp Normal file
View File

@@ -0,0 +1,338 @@
#include "tracer_base/tracer_base.hpp"
#include <string>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <ratio>
#include <thread>
#include "stopwatch/stopwatch.h"
namespace wescore
{
TracerBase::~TracerBase()
{
if (serial_connected_)
serial_if_->close();
if (cmd_thread_.joinable())
cmd_thread_.join();
}
void TracerBase::Connect(std::string dev_name)
{
// if (baud_rate == 0) {
ConfigureCANBus(dev_name);
// } else {
// ConfigureSerial(dev_name, baud_rate);
// if (!serial_connected_)
// std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
// }
}
void TracerBase::Disconnect()
{
if (serial_connected_)
{
if (serial_if_->is_open())
serial_if_->close();
}
}
void TracerBase::ConfigureCANBus(const std::string &can_if_name)
{
can_if_ = std::make_shared<ASyncCAN>(can_if_name);
can_if_->set_receive_callback(std::bind(&TracerBase::ParseCANFrame, this, std::placeholders::_1));
can_connected_ = true;
}
void TracerBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate)
{
serial_if_ = std::make_shared<ASyncSerial>(uart_name, baud_rate);
serial_if_->open();
if (serial_if_->is_open())
serial_connected_ = true;
serial_if_->set_receive_callback(std::bind(&TracerBase::ParseUARTBuffer, this,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3));
}
void TracerBase::StartCmdThread()
{
current_motion_cmd_.linear_velocity = 0;
current_motion_cmd_.angular_velocity = 0;
current_motion_cmd_.fault_clear_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT;
cmd_thread_ = std::thread(std::bind(&TracerBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void TracerBase::SendMotionCmd(uint8_t count)
{
// motion control message
TracerMessage m_msg;
m_msg.type = TracerMotionCmdMsg;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
else if (serial_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
motion_cmd_mutex_.lock();
m_msg.body.motion_cmd_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.body.motion_cmd_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity;
m_msg.body.motion_cmd_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity;
motion_cmd_mutex_.unlock();
m_msg.body.motion_cmd_msg.data.cmd.reserved0 = 0;
m_msg.body.motion_cmd_msg.data.cmd.reserved1 = 0;
m_msg.body.motion_cmd_msg.data.cmd.count = count;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a complete serial frame
if (can_connected_)
{
// send to can bus
can_frame m_frame;
EncodeTracerMsgToCAN(&m_msg, &m_frame);
can_if_->send_frame(m_frame);
}
else
{
// TODO
// send to serial port
// EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
// serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
}
}
void TracerBase::SendLightCmd(uint8_t count)
{
TracerMessage l_msg;
l_msg.type = TracerLightControlMsg;
light_cmd_mutex_.lock();
if (light_ctrl_enabled_)
{
l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL;
l_msg.body.light_control_msg.data.cmd.front_light_mode = static_cast<uint8_t>(current_light_cmd_.front_mode);
l_msg.body.light_control_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value;
l_msg.body.light_control_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode);
l_msg.body.light_control_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;
// std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " << l_msg.data.cmd.front_light_custom << " , "
// << l_msg.data.cmd.rear_light_mode << " , " << l_msg.data.cmd.rear_light_custom << std::endl;
// std::cout << "light cmd generated" << std::endl;
}
else
{
l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL;
l_msg.body.light_control_msg.data.cmd.front_light_mode = LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.front_light_custom = 0;
l_msg.body.light_control_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0;
}
light_ctrl_requested_ = false;
light_cmd_mutex_.unlock();
l_msg.body.light_control_msg.data.cmd.reserved0 = 0;
l_msg.body.light_control_msg.data.cmd.count = count;
if (can_connected_)
l_msg.body.light_control_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a complete serial frame
if (can_connected_)
{
// send to can bus
can_frame l_frame;
EncodeTracerMsgToCAN(&l_msg, &l_frame);
can_if_->send_frame(l_frame);
}
// else
// {
// // send to serial port
// EncodeTracerMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);
// serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
// }
// std::cout << "cmd: " << static_cast<int>(l_msg.data.cmd.front_light_mode) << " , " << static_cast<int>(l_msg.data.cmd.front_light_custom) << " , "
// << static_cast<int>(l_msg.data.cmd.rear_light_mode) << " , " << static_cast<int>(l_msg.data.cmd.rear_light_custom) << std::endl;
// std::cout << "can: ";
// for (int i = 0; i < 8; ++i)
// std::cout << static_cast<int>(l_frame.data[i]) << " ";
// std::cout << "uart: ";
// for (int i = 0; i < tx_cmd_len_; ++i)
// std::cout << static_cast<int>(tx_buffer_[i]) << " ";
// std::cout << std::endl;
}
void TracerBase::ControlLoop(int32_t period_ms)
{
StopWatch ctrl_sw;
uint8_t cmd_count = 0;
uint8_t light_cmd_count = 0;
while (true)
{
ctrl_sw.tic();
// motion control message
SendMotionCmd(cmd_count++);
// check if there is request for light control
if (light_ctrl_requested_)
SendLightCmd(light_cmd_count++);
ctrl_sw.sleep_until_ms(period_ms);
// std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << std::endl;
}
}
TracerState TracerBase::GetTracerState()
{
std::lock_guard<std::mutex> guard(tracer_state_mutex_);
return tracer_state_;
}
void TracerBase::SetMotionCommand(double linear_vel, double angular_vel, TracerMotionCmd::FaultClearFlag fault_clr_flag)
{
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_)
StartCmdThread();
if (linear_vel < TracerMotionCmd::min_linear_velocity)
linear_vel = TracerMotionCmd::min_linear_velocity;
if (linear_vel > TracerMotionCmd::max_linear_velocity)
linear_vel = TracerMotionCmd::max_linear_velocity;
if (angular_vel < TracerMotionCmd::min_angular_velocity)
angular_vel = TracerMotionCmd::min_angular_velocity;
if (angular_vel > TracerMotionCmd::max_angular_velocity)
angular_vel = TracerMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(linear_vel / TracerMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(angular_vel / TracerMotionCmd::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
}
void TracerBase::SetLightCommand(TracerLightCmd cmd)
{
if (!cmd_thread_started_)
StartCmdThread();
std::lock_guard<std::mutex> guard(light_cmd_mutex_);
current_light_cmd_ = cmd;
light_ctrl_enabled_ = true;
light_ctrl_requested_ = true;
}
void TracerBase::DisableLightCmdControl()
{
std::lock_guard<std::mutex> guard(light_cmd_mutex_);
light_ctrl_enabled_ = false;
light_ctrl_requested_ = true;
}
void TracerBase::ParseCANFrame(can_frame *rx_frame)
{
// validate checksum, discard frame if fails
if (!rx_frame->data[7] == CalcTracerCANChecksum(rx_frame->can_id, rx_frame->data, rx_frame->can_dlc))
{
std::cerr << "ERROR: checksum mismatch, discard frame with id " << rx_frame->can_id << std::endl;
return;
}
// otherwise, update robot state with new frame
TracerMessage status_msg;
DecodeTracerMsgFromCAN(rx_frame, &status_msg);
NewStatusMsgReceivedCallback(status_msg);
}
void TracerBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
{
// std::cout << "bytes received from serial: " << bytes_received << std::endl;
// serial_parser_.PrintStatistics();
// serial_parser_.ParseBuffer(buf, bytes_received);
// TODO
// TracerMessage status_msg;
// for (int i = 0; i < bytes_received; ++i)
// {
// if (DecodeTracerMsgFromUART(buf[i], &status_msg))
// NewStatusMsgReceivedCallback(status_msg);
// }
}
void TracerBase::NewStatusMsgReceivedCallback(const TracerMessage &msg)
{
// std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(tracer_state_mutex_);
UpdateTracerState(msg, tracer_state_);
}
void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState &state)
{
switch (status_msg.type)
{
case TracerMotionStatusMsg:
{
// std::cout << "motion control feedback received" << std::endl;
const MotionStatusMessage &msg = status_msg.body.motion_status_msg;
state.linear_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.linear_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.linear_velocity.high_byte) << 8) / 1000.0;
state.angular_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.angular_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.angular_velocity.high_byte) << 8) / 1000.0;
break;
}
case TracerLightStatusMsg:
{
// std::cout << "light control feedback received" << std::endl;
const LightStatusMessage &msg = status_msg.body.light_status_msg;
if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL)
state.light_control_enabled = false;
else
state.light_control_enabled = true;
state.front_light_state.mode = msg.data.status.front_light_mode;
state.front_light_state.custom_value = msg.data.status.front_light_custom;
state.rear_light_state.mode = msg.data.status.rear_light_mode;
state.rear_light_state.custom_value = msg.data.status.rear_light_custom;
break;
}
case TracerSystemStatusMsg:
{
// std::cout << "system status feedback received" << std::endl;
const SystemStatusMessage &msg = status_msg.body.system_status_msg;
state.control_mode = msg.data.status.control_mode;
state.base_state = msg.data.status.base_state;
state.battery_voltage = (static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) | static_cast<uint16_t>(msg.data.status.battery_voltage.high_byte) << 8) / 10.0;
state.fault_code = (static_cast<uint16_t>(msg.data.status.fault_code.low_byte) | static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8);
break;
}
case TracerMotorDriverStatusMsg:
{
// std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverStatusMessage &msg = status_msg.body.motor_driver_status_msg;
for (int i = 0; i < 4; ++i)
{
state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].current = (static_cast<uint16_t>(msg.data.status.current.low_byte) | static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) / 10.0;
state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].rpm = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.rpm.low_byte) | static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].temperature = msg.data.status.temperature;
}
break;
}
}
}
} // namespace wescore

16
src/tracer_sdk/tracer_base/tests/CMakeLists.txt Normal file
View File

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

0
src/hunter_sdk/hunter_base/tests/test_scout_base.cpp → src/tracer_sdk/tracer_base/tests/test_tracer_base.cpp
View File

9
src/tracer_sdk/tracer_protocol/CMakeLists.txt Executable file
View File

@@ -0,0 +1,9 @@
## Add libraries
set(TRACER_PROTOCOL_SRC
src/tracer_can_parser.c
)
add_library(tracer_protocol STATIC ${TRACER_PROTOCOL_SRC})
target_include_directories(tracer_protocol PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)

42
src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_can_parser.h Normal file
View File

@@ -0,0 +1,42 @@
/*
* tracer_can_parser.h
*
* Created on: Apr 14, 2020 10:52
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_CAN_PARSER_H
#define TRACER_CAN_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include "tracer_protocol/tracer_protocol.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
bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg);
void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame);
uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* TRACER_CAN_PARSER_H */

259
src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_protocol.h Normal file
View File

@@ -0,0 +1,259 @@
/*
* tracer_protocol.h
*
* Created on: Apr 14, 2020 10:34
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef TRACER_PROTOCOL_H
#define TRACER_PROTOCOL_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#define TRACER_CMD_BUF_LEN 32
#define TRACER_STATUS_BUF_LEN 32
#define TRACER_FRAME_SIZE 13
#define TRACER_MOTOR1_ID ((uint8_t)0x00)
#define TRACER_MOTOR2_ID ((uint8_t)0x01)
// CAN Definitions
#define CAN_MSG_MOTION_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_STATUS_ID ((uint32_t)0x131)
#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151)
#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x201)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x202)
/*--------------------- Control/State Constants ------------------------*/
// Motion Control
#define CTRL_MODE_REMOTE ((uint8_t)0x00)
#define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
#define CTRL_MODE_CMD_UART ((uint8_t)0x02)
#define CTRL_MODE_COMMANDED ((uint8_t)0x03)
#define FAULT_CLR_NONE ((uint8_t)0x00)
#define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
#define FAULT_CLR_BAT_OVER_VOL ((uint8_t)0x02)
#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x03)
#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x04)
#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x05)
#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x06)
#define FAULT_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07)
#define FAULT_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08)
// Light Control
#define LIGHT_DISABLE_CTRL ((uint8_t)0x00)
#define LIGHT_ENABLE_CTRL ((uint8_t)0x01)
#define LIGHT_MODE_CONST_OFF ((uint8_t)0x00)
#define LIGHT_MODE_CONST_ON ((uint8_t)0x01)
#define LIGHT_MODE_BREATH ((uint8_t)0x02)
#define LIGHT_MODE_CUSTOM ((uint8_t)0x03)
// System Status Feedback
#define BASE_STATE_NORMAL ((uint8_t)0x00)
#define BASE_STATE_ESTOP ((uint8_t)0x01)
#define BASE_STATE_EXCEPTION ((uint8_t)0x02)
#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100)
#define FAULT_FRONT_STEER_ENCODER_F ((uint16_t)0x0200)
#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x0400)
#define FAULT_HIGH_BYTE_RESERVED1 ((uint16_t)0x0800)
#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x1000)
#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x2000)
#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x4000)
#define FAULT_HIGH_BYTE_RESERVED5 ((uint16_t)0x8000)
#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001)
#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002)
#define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004)
#define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008)
#define FAULT_RESERVED1 ((uint16_t)0x0010)
#define FAULT_RESERVED2 ((uint16_t)0x0020)
#define FAULT_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040)
#define FAULT_MOTOR_OVERCURRENT_F ((uint16_t)0x0080)
/*-------------------- Control/Feedback Messages -----------------------*/
/* No padding in the struct */
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#pragma pack(push, 1)
// Note: id could be different for UART and CAN protocol
// Motion Control
typedef struct {
union
{
struct
{
uint8_t control_mode;
uint8_t fault_clear_flag;
int8_t linear_velocity_cmd;
int8_t angular_velocity_cmd;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} MotionCmdMessage;
typedef struct {
union
{
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} linear_velocity;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotionStatusMessage;
// System Status Feedback
typedef struct {
union
{
struct
{
uint8_t base_state;
uint8_t control_mode;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} battery_voltage;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} fault_code;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
// Light Control
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} LightControlMessage;
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} LightStatusMessage;
// Motor Driver Feedback
typedef struct
{
uint8_t motor_id;
union {
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} current;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} rpm;
int8_t temperature;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotorDriverStatusMessage;
// For convenience to access status/control message
typedef enum
{
TracerMsgNone = 0x00,
// status messages
TracerMotionStatusMsg = 0x01,
TracerLightStatusMsg = 0x02,
TracerSystemStatusMsg = 0x03,
TracerMotorDriverStatusMsg = 0x04,
// control messages
TracerMotionCmdMsg = 0x21,
TracerLightControlMsg = 0x22
} TracerMsgType;
typedef struct
{
TracerMsgType type;
union {
// status messages
MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
// control messages
MotionCmdMessage motion_cmd_msg;
LightControlMessage light_control_msg;
} body;
} TracerMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* TRACER_PROTOCOL_H */

118
src/tracer_sdk/tracer_protocol/src/tracer_can_parser.c Normal file
View File

@@ -0,0 +1,118 @@
/*
* tracer_can_parser.c
*
* Created on: Apr 14, 2020 10:35
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "tracer_protocol/tracer_can_parser.h"
#include "string.h"
static void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame);
bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg)
{
msg->type = TracerMsgNone;
switch (rx_frame->can_id)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_STATUS_ID:
{
msg->type = TracerMotionStatusMsg;
memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_SYSTEM_STATUS_STATUS_ID:
{
msg->type = TracerSystemStatusMsg;
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR1_DRIVER_STATUS_ID:
{
msg->type = TracerMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR1_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR2_DRIVER_STATUS_ID:
{
msg->type = TracerMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR2_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CMD_ID:
{
msg->type = TracerMotionCmdMsg;
memcpy(msg->body.motion_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
return true;
}
void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame)
{
switch (msg->type)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case TracerMotionStatusMsg:
{
tx_frame->can_id = CAN_MSG_MOTION_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case TracerSystemStatusMsg:
{
tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case TracerMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR1_ID)
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR2_ID)
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case TracerMotionCmdMsg:
{
EncodeTracerMotionControlMsgToCAN(&(msg->body.motion_cmd_msg), tx_frame);
break;
}
default:
break;
}
tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_MOTION_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
{
uint8_t checksum = 0x00;
checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
for (int i = 0; i < (dlc - 1); ++i)
checksum += data[i];
return checksum;
}