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 time stamp to robot states

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-15 15:25:49 +08:00
parent e96b078c05
commit f74ee40fc3
32 changed files with 248 additions and 328 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@@ -86,6 +86,9 @@ add_library(${PROJECT_NAME}
src/async_port/async_serial.cpp
src/async_port/async_can.cpp
########################
## utilities
src/utilities/protocol_detector.cpp
########################
## public interface to access robot
src/mobile_robot/scout_robot.cpp
src/mobile_robot/hunter_robot.cpp

2
demo/CMakeLists.txt
View File

@@ -4,3 +4,5 @@ add_subdirectory(tracer_demo)
add_subdirectory(ranger_demo)
add_subdirectory(bunker_demo)
add_subdirectory(hunter_demo)
add_subdirectory(utils_demo)

1
demo/bunker_demo/CMakeLists.txt
View File

@@ -1,3 +1,2 @@
# tests
add_executable(demo_bunker_robot bunker_robot_demo.cpp)
target_link_libraries(demo_bunker_robot ugv_sdk)

1
demo/hunter_demo/CMakeLists.txt
View File

@@ -1,3 +1,2 @@
# tests
add_executable(demo_hunter_robot hunter_robot_demo.cpp)
target_link_libraries(demo_hunter_robot ugv_sdk)

92
demo/hunter_demo/hunter_demo.cpp
View File

@@ -1,92 +0,0 @@
/*
* demo_hunter_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/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;
}

2
demo/ranger_demo/CMakeLists.txt
View File

@@ -1,2 +1,2 @@
add_executable(demo_ranger_robot ranger_demo.cpp)
add_executable(demo_ranger_robot ranger_robot_demo.cpp)
target_link_libraries(demo_ranger_robot ugv_sdk)

0
demo/ranger_demo/ranger_demo.cpp → demo/ranger_demo/ranger_robot_demo.cpp
View File

4
demo/scout_demo/CMakeLists.txt
View File

@@ -1,6 +1,2 @@
# 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)

57
demo/scout_demo/scout_robot_demo.cpp
View File

@@ -13,34 +13,38 @@
#include <iostream>
#include "ugv_sdk/mobile_robot/scout_robot.hpp"
#include "ugv_sdk/utilities/protocol_detector.hpp"
using namespace westonrobot;
int main(int argc, char **argv) {
std::string protocol_version;
std::string device_name;
if (argc == 3) {
protocol_version = {argv[1]};
device_name = {argv[2]};
std::cout << "Use protocol " << protocol_version << " on interface "
<< device_name << std::endl;
if (argc == 2) {
device_name = {argv[1]};
std::cout << "Selected interface " << device_name << std::endl;
} else {
std::cout << "Usage: app_scout_demo <protocol-version> <interface>"
<< std::endl
<< "Example 1: ./app_scout_demo v1 can0" << std::endl;
<< "Example 1: ./app_scout_demo can0" << std::endl;
return -1;
}
std::unique_ptr<ScoutRobot> scout;
if (protocol_version == "v1") {
ProtocolDectctor detector;
detector.Connect("can0");
auto proto = detector.DetectProtocolVersion(5);
if (proto == ProtocolVersion::AGX_V1) {
std::cout << "Detected protocol: AGX_V1" << std::endl;
scout = std::unique_ptr<ScoutRobot>(
new ScoutRobot(ProtocolVersion::AGX_V1, true));
} else if (protocol_version == "v2") {
} else if (proto == ProtocolVersion::AGX_V2) {
std::cout << "Detected protocol: AGX_V2" << std::endl;
scout = std::unique_ptr<ScoutRobot>(
new ScoutRobot(ProtocolVersion::AGX_V2, true));
} else {
std::cout << "Error: invalid protocol version string" << std::endl;
std::cout << "Detected protocol: UNKONWN" << std::endl;
return -1;
}
@@ -51,9 +55,6 @@ int main(int argc, char **argv) {
if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V2) {
scout->EnableCommandedMode();
std::cout << "Protocol version 2" << std::endl;
} else {
std::cout << "Protocol version 1" << std::endl;
}
// light control
@@ -66,11 +67,9 @@ int main(int argc, char **argv) {
std::cout << "Light: breath" << std::endl;
scout->SetLightCommand(BREATH, 0, BREATH, 0);
sleep(3);
std::cout << "Light: custom 30-80" << std::endl;
scout->SetLightCommand(CUSTOM, 30, CUSTOM, 80);
std::cout << "Light: custom 30-40" << std::endl;
scout->SetLightCommand(CUSTOM, 30, CUSTOM, 40);
sleep(3);
// std::cout << "Light: diabled cmd control" << std::endl;
// scout->DisableLightControl();
scout->SetLightCommand(CONST_OFF, 0, CONST_OFF, 0);
int count = 0;
@@ -88,11 +87,17 @@ int main(int argc, char **argv) {
<< " , vehicle state: "
<< static_cast<int>(state.system_state.vehicle_state)
<< " , error code: " << std::hex << state.system_state.error_code
<< std::dec
<< ", 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 << "core state age (ms): "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
AgxMsgRefClock::now() - state.time_stamp)
.count()
<< std::endl;
auto actuator = scout->GetActuatorState();
if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V1) {
@@ -104,7 +109,25 @@ int main(int argc, char **argv) {
actuator.actuator_state[i].driver_temp,
actuator.actuator_state[i].motor_temp);
}
std::cout << "actuator state age (ms): "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
AgxMsgRefClock::now() - actuator.time_stamp)
.count()
<< std::endl;
} else {
for (int i = 0; i < 4; ++i) {
printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n",
actuator.actuator_hs_state[i].motor_id,
actuator.actuator_hs_state[i].current,
actuator.actuator_hs_state[i].rpm,
actuator.actuator_ls_state[i].driver_temp,
actuator.actuator_ls_state[i].motor_temp);
}
std::cout << "actuator state age (ms): "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
AgxMsgRefClock::now() - actuator.time_stamp)
.count()
<< std::endl;
}
std::cout << "-------------------------------" << std::endl;

75
demo/scout_demo/scout_v2_demo.cpp
View File

@@ -1,75 +0,0 @@
/*
* demo_scout_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/details/robot_base/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;
}
ScoutBaseV2 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;
}

99
demo/tracer_demo/tracer_demo.cpp
View File

@@ -1,99 +0,0 @@
/*
* demo_tracer_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/protocol_v2/tracer_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_tracer_demo <interface>" << std::endl
<< "Example 1: ./app_tracer_demo can0" << std::endl;
return -1;
}
TracerBaseV2 tracer;
tracer.Connect(device_name);
tracer.EnableCommandedMode();
// tracer.DisableTimeout();
// light control
std::cout << "Light: const off" << std::endl;
tracer.SetLightCommand({CONST_OFF, 0});
// usleep(50000);
sleep(3);
std::cout << "Light: const on" << std::endl;
tracer.SetLightCommand({CONST_ON, 0});
// usleep(50000);
sleep(3);
std::cout << "Light: breath" << std::endl;
tracer.SetLightCommand({BREATH, 0});
// usleep(50000);
sleep(8);
std::cout << "Light: custom 90-80" << std::endl;
tracer.SetLightCommand({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.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);
sleep(1);
++count;
}
return 0;
}

2
demo/utils_demo/CMakeLists.txt Normal file
View File

@@ -0,0 +1,2 @@
add_executable(demo_protocol_detector demo_protocol_detector.cpp)
target_link_libraries(demo_protocol_detector ugv_sdk)

32
demo/utils_demo/demo_protocol_detector.cpp Normal file
View File

@@ -0,0 +1,32 @@
/*
* demo_protocol_detect.cpp
*
* Created on: Jul 15, 2021 14:10
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include <memory>
#include <iostream>
#include "ugv_sdk/utilities/protocol_detector.hpp"
using namespace westonrobot;
int main(int argc, char **argv) {
ProtocolDectctor detector;
detector.Connect("can0");
auto proto = detector.DetectProtocolVersion(5);
if (proto == ProtocolVersion::AGX_V1) {
std::cout << "Detected protocol: AGX_V1" << std::endl;
} else if (proto == ProtocolVersion::AGX_V2) {
std::cout << "Detected protocol: AGX_V2" << std::endl;
} else {
std::cout << "Detected protocol: UNKONWN" << std::endl;
}
return 0;
}

4
include/ugv_sdk/details/interface/agilex_message.h
View File

@@ -119,8 +119,8 @@ typedef struct {
typedef struct {
uint8_t motor_id;
float driver_voltage;
float driver_temperature;
int8_t motor_temperature;
float driver_temp;
float motor_temp;
uint8_t driver_state;
} ActuatorLSStateMessage;

3
include/ugv_sdk/details/interface/bunker_interface.hpp
View File

@@ -13,9 +13,12 @@
#include <string>
#include "ugv_sdk/details/interface/agilex_message.h"
#include "ugv_sdk/details/interface/robot_common_interface.hpp"
namespace westonrobot {
struct BunkerCoreState {
AgxMsgTimeStamp time_stamp;
SystemStateMessage system_state;
MotionStateMessage motion_state;
RcStateMessage rc_state;

6
include/ugv_sdk/details/interface/hunter_interface.hpp
View File

@@ -13,16 +13,20 @@
#include <string>
#include "ugv_sdk/details/interface/agilex_message.h"
#include "ugv_sdk/details/interface/robot_common_interface.hpp"
namespace westonrobot {
struct HunterCoreState {
AgxMsgTimeStamp time_stamp;
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
RcStateMessage rc_state;
};
struct HunterActuatorState {
AgxMsgTimeStamp time_stamp;
// actuator state
ActuatorHSStateMessage actuator_hs_state[3];
ActuatorLSStateMessage actuator_ls_state[3];

5
include/ugv_sdk/details/interface/ranger_interface.hpp
View File

@@ -13,10 +13,13 @@
#include <string>
#include "ugv_sdk/details/interface/agilex_message.h"
#include "ugv_sdk/details/interface/robot_common_interface.hpp"
namespace westonrobot {
struct RangerCoreState {
// system state
AgxMsgTimeStamp time_stamp;
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
@@ -27,6 +30,8 @@ struct RangerCoreState {
};
struct RangerActuatorState {
AgxMsgTimeStamp time_stamp;
ActuatorHSStateMessage actuator_hs_state[8];
ActuatorLSStateMessage actuator_ls_state[8];
};

8
include/ugv_sdk/details/interface/robot_common_interface.hpp
View File

@@ -11,6 +11,7 @@
#define ROBOT_INTERFACE_HPP
#include <string>
#include <chrono>
#include "ugv_sdk/details/interface/agilex_message.h"
#include "ugv_sdk/details/interface/parser_interface.hpp"
@@ -18,7 +19,12 @@
#define AGX_MAX_ACTUATOR_NUM 8
namespace westonrobot {
using AgxMsgRefClock = std::chrono::steady_clock;
using AgxMsgTimeStamp = std::chrono::time_point<AgxMsgRefClock>;
struct CoreStateMsgGroup {
AgxMsgTimeStamp time_stamp;
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
@@ -27,6 +33,8 @@ struct CoreStateMsgGroup {
};
struct ActuatorStateMsgGroup {
AgxMsgTimeStamp time_stamp;
ActuatorHSStateMessage actuator_hs_state[AGX_MAX_ACTUATOR_NUM]; // v2 only
ActuatorLSStateMessage actuator_ls_state[AGX_MAX_ACTUATOR_NUM]; // v2 only
ActuatorStateMessageV1 actuator_state[AGX_MAX_ACTUATOR_NUM]; // v1 only

5
include/ugv_sdk/details/interface/scout_interface.hpp
View File

@@ -13,9 +13,12 @@
#include <string>
#include "ugv_sdk/details/interface/agilex_message.h"
#include "ugv_sdk/details/interface/robot_common_interface.hpp"
namespace westonrobot {
struct ScoutCoreState {
AgxMsgTimeStamp time_stamp;
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
@@ -23,6 +26,8 @@ struct ScoutCoreState {
};
struct ScoutActuatorState {
AgxMsgTimeStamp time_stamp;
// actuator state
// - for v2 robots only
ActuatorHSStateMessage actuator_hs_state[4];

5
include/ugv_sdk/details/interface/tracer_interface.hpp
View File

@@ -13,9 +13,12 @@
#include <string>
#include "ugv_sdk/details/interface/agilex_message.h"
#include "ugv_sdk/details/interface/robot_common_interface.hpp"
namespace westonrobot {
struct TracerCoreState {
AgxMsgTimeStamp time_stamp;
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
@@ -23,6 +26,8 @@ struct TracerCoreState {
};
struct TracerActuatorState {
AgxMsgTimeStamp time_stamp;
// actuator state
ActuatorHSStateMessage actuator_hs_state[2];
ActuatorLSStateMessage actuator_ls_state[2];

15
include/ugv_sdk/details/robot_base/agilex_base.hpp
View File

@@ -22,7 +22,6 @@
#include <mutex>
#include <atomic>
#include "ugv_sdk/details/stopwatch.hpp"
#include "ugv_sdk/details/async_port/async_can.hpp"
#include "ugv_sdk/details/interface/robot_common_interface.hpp"
@@ -79,8 +78,9 @@ class AgilexBase : public RobotCommonInterface {
msg.body.motion_command_msg.steering_angle = steering_angle;
}
std::cout << "sending motion cmd: " << linear_vel << "," << angular_vel
<< std::endl;
// std::cout << "sending motion cmd: " << linear_vel << "," <<
// angular_vel
// << std::endl;
// send to can bus
can_frame frame;
@@ -195,27 +195,32 @@ class AgilexBase : public RobotCommonInterface {
switch (status_msg.type) {
case AgxMsgSystemState: {
// std::cout << "system status feedback received" << std::endl;
core_state_msgs_.time_stamp = AgxMsgRefClock::now();
core_state_msgs_.system_state = status_msg.body.system_state_msg;
break;
}
case AgxMsgMotionState: {
// std::cout << "motion control feedback received" << std::endl;
core_state_msgs_.time_stamp = AgxMsgRefClock::now();
core_state_msgs_.motion_state = status_msg.body.motion_state_msg;
break;
}
case AgxMsgLightState: {
// std::cout << "light control feedback received" << std::endl;
core_state_msgs_.time_stamp = AgxMsgRefClock::now();
core_state_msgs_.light_state = status_msg.body.light_state_msg;
break;
}
case AgxMsgMotionModeState: {
// std::cout << "motion mode feedback received" << std::endl;
core_state_msgs_.time_stamp = AgxMsgRefClock::now();
core_state_msgs_.motion_mode_state =
status_msg.body.motion_mode_state_msg;
break;
}
case AgxMsgRcState: {
// std::cout << "rc feedback received" << std::endl;
core_state_msgs_.time_stamp = AgxMsgRefClock::now();
core_state_msgs_.rc_state = status_msg.body.rc_state_msg;
break;
}
@@ -229,6 +234,7 @@ class AgilexBase : public RobotCommonInterface {
switch (status_msg.type) {
case AgxMsgActuatorHSState: {
// std::cout << "actuator hs feedback received" << std::endl;
actuator_state_msgs_.time_stamp = AgxMsgRefClock::now();
actuator_state_msgs_
.actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] =
status_msg.body.actuator_hs_state_msg;
@@ -236,7 +242,7 @@ class AgilexBase : public RobotCommonInterface {
}
case AgxMsgActuatorLSState: {
// std::cout << "actuator ls feedback received" << std::endl;
actuator_state_msgs_.time_stamp = AgxMsgRefClock::now();
actuator_state_msgs_
.actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
status_msg.body.actuator_ls_state_msg;
@@ -244,6 +250,7 @@ class AgilexBase : public RobotCommonInterface {
}
case AgxMsgActuatorStateV1: {
// std::cout << "actuator v1 feedback received" << std::endl;
actuator_state_msgs_.time_stamp = AgxMsgRefClock::now();
actuator_state_msgs_
.actuator_state[status_msg.body.v1_actuator_state_msg.motor_id] =
status_msg.body.v1_actuator_state_msg;

1
include/ugv_sdk/details/robot_base/bunker_base.hpp
View File

@@ -41,6 +41,7 @@ class BunkerBase : public AgilexBase<ParserType>, public BunkerInterface {
auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
BunkerCoreState bunker_state;
bunker_state.time_stamp = state.time_stamp;
bunker_state.system_state = state.system_state;
bunker_state.motion_state = state.motion_state;
bunker_state.rc_state = state.rc_state;

38
include/ugv_sdk/details/robot_base/hunter_base.hpp
View File

@@ -15,12 +15,12 @@
#include <thread>
#include <mutex>
#include "ugv_sdk/details/interface/scout_interface.hpp"
#include "ugv_sdk/details/interface/hunter_interface.hpp"
#include "ugv_sdk/details/robot_base/agilex_base.hpp"
namespace westonrobot {
template <typename ParserType>
class HunterBase : public AgilexBase<ParserType>, public ScoutInterface {
class HunterBase : public AgilexBase<ParserType>, public HunterInterface {
public:
HunterBase() : AgilexBase<ParserType>(){};
~HunterBase() = default;
@@ -36,33 +36,29 @@ class HunterBase : public AgilexBase<ParserType>, public ScoutInterface {
angular_vel);
}
void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value) override {
AgilexBase<ParserType>::SendLightCommand(f_mode, f_value, r_mode, r_value);
}
// get robot state
ScoutCoreState GetRobotState() override {
HunterCoreState GetRobotState() override {
auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
ScoutCoreState scout_state;
scout_state.system_state = state.system_state;
scout_state.motion_state = state.motion_state;
scout_state.light_state = state.light_state;
scout_state.rc_state = state.rc_state;
return scout_state;
HunterCoreState hunter_state;
hunter_state.time_stamp = state.time_stamp;
hunter_state.system_state = state.system_state;
hunter_state.motion_state = state.motion_state;
hunter_state.rc_state = state.rc_state;
return hunter_state;
}
ScoutActuatorState GetActuatorState() override {
HunterActuatorState GetActuatorState() override {
auto actuator = AgilexBase<ParserType>::GetActuatorStateMsgGroup();
ScoutActuatorState scout_actuator;
for (int i = 0; i < 4; ++i) {
scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];
scout_actuator.actuator_state[i] = actuator.actuator_state[i];
HunterActuatorState hunter_actuator;
hunter_actuator.time_stamp = actuator.time_stamp;
for (int i = 0; i < 3; ++i) {
hunter_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
hunter_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];
hunter_actuator.actuator_state[i] = actuator.actuator_state[i];
}
return scout_actuator;
return hunter_actuator;
}
};
} // namespace westonrobot

2
include/ugv_sdk/details/robot_base/ranger_base.hpp
View File

@@ -55,6 +55,7 @@ class RangerBase : public AgilexBase<ProtocolV2Parser>, public RangerInterface {
auto state = AgilexBase<ProtocolV2Parser>::GetRobotCoreStateMsgGroup();
RangerCoreState ranger_state;
ranger_state.time_stamp = state.time_stamp;
ranger_state.system_state = state.system_state;
ranger_state.motion_state = state.motion_state;
ranger_state.light_state = state.light_state;
@@ -66,6 +67,7 @@ class RangerBase : public AgilexBase<ProtocolV2Parser>, public RangerInterface {
auto actuator = AgilexBase<ProtocolV2Parser>::GetActuatorStateMsgGroup();
RangerActuatorState ranger_actuator;
ranger_actuator.time_stamp = actuator.time_stamp;
for (int i = 0; i < 8; ++i) {
ranger_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
ranger_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];

2
include/ugv_sdk/details/robot_base/scout_base.hpp
View File

@@ -50,6 +50,7 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
ScoutCoreState scout_state;
scout_state.time_stamp = state.time_stamp;
scout_state.system_state = state.system_state;
scout_state.motion_state = state.motion_state;
scout_state.light_state = state.light_state;
@@ -61,6 +62,7 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
auto actuator = AgilexBase<ParserType>::GetActuatorStateMsgGroup();
ScoutActuatorState scout_actuator;
scout_actuator.time_stamp = actuator.time_stamp;
for (int i = 0; i < 4; ++i) {
scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];

4
include/ugv_sdk/details/robot_base/tracer_base.hpp
View File

@@ -31,7 +31,7 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>,
void Connect(std::string can_name) override {
AgilexBase<ProtocolV2Parser>::Connect(can_name);
}
void Connect(std::string uart_name, uint32_t baudrate) override {
// TODO
}
@@ -52,6 +52,7 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>,
auto state = AgilexBase<ProtocolV2Parser>::GetRobotCoreStateMsgGroup();
TracerCoreState tracer_state;
tracer_state.time_stamp = state.time_stamp;
tracer_state.system_state = state.system_state;
tracer_state.motion_state = state.motion_state;
tracer_state.light_state = state.light_state;
@@ -63,6 +64,7 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>,
auto actuator = AgilexBase<ProtocolV2Parser>::GetActuatorStateMsgGroup();
TracerActuatorState tracer_actuator;
tracer_actuator.time_stamp = actuator.time_stamp;
for (int i = 0; i < 2; ++i) {
tracer_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
tracer_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];

35
include/ugv_sdk/utilities/protocol_detector.hpp Normal file
View File

@@ -0,0 +1,35 @@
/*
* protocol_detector.hpp
*
* Created on: Jul 15, 2021 14:03
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#ifndef PROTOCOL_DETECTOR_HPP
#define PROTOCOL_DETECTOR_HPP
#include <atomic>
#include "ugv_sdk/details/async_port/async_can.hpp"
#include "ugv_sdk/details/interface/parser_interface.hpp"
namespace westonrobot {
class ProtocolDectctor {
public:
void Connect(std::string can_name);
void Connect(std::string uart_name, uint32_t baudrate);
ProtocolVersion DetectProtocolVersion(uint32_t timeout_sec);
private:
std::shared_ptr<AsyncCAN> can_;
void ParseCANFrame(can_frame *rx_frame);
std::atomic<bool> msg_v1_detected_;
std::atomic<bool> msg_v2_detected_;
};
} // namespace westonrobot
#endif /* PROTOCOL_DETECTOR_HPP */

2
include/ugv_sdk/details/stopwatch.hpp → include/ugv_sdk/utilities/stopwatch.hpp
View File

@@ -9,7 +9,7 @@
* [2] https://github.com/rxdu/stopwatch
*
* Copyright (c) 2019 sailormoon <http://unlicense.org>
* Copyright (c) 2020 Ruixiang Du (rdu)
* Copyright (c) 2020 Weston Robot Pte. Ltd.
*
* License: <http://unlicense.org>
*/

2
src/protocol_v1/agilex_msg_parser_v1.c
View File

@@ -18,7 +18,7 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
// if checksum not correct
if (!CalcCanFrameChecksumV1(rx_frame->can_id, (uint8_t *)rx_frame->data,
rx_frame->can_dlc)) {
printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id);
// printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id);
return false;
}

10
src/protocol_v2/agilex_msg_parser_v2.c
View File

@@ -182,11 +182,11 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
((uint16_t)(frame->driver_voltage.low_byte) |
(uint16_t)(frame->driver_voltage.high_byte) << 8) *
0.1;
msg->body.actuator_ls_state_msg.driver_temperature =
(int16_t)((uint16_t)(frame->driver_temperature.low_byte) |
(uint16_t)(frame->driver_temperature.high_byte) << 8);
msg->body.actuator_ls_state_msg.motor_temperature =
frame->motor_temperature;
msg->body.actuator_ls_state_msg.driver_temp =
(int16_t)((uint16_t)(frame->driver_temp.low_byte) |
(uint16_t)(frame->driver_temp.high_byte) << 8);
msg->body.actuator_ls_state_msg.motor_temp =
frame->motor_temp;
msg->body.actuator_ls_state_msg.driver_state = frame->driver_state;
break;
}

4
src/protocol_v2/agilex_protocol_v2.h
View File

@@ -242,8 +242,8 @@ typedef struct {
typedef struct {
struct16_t driver_voltage;
struct16_t driver_temperature;
int8_t motor_temperature;
struct16_t driver_temp;
int8_t motor_temp;
uint8_t driver_state;
uint8_t reserved0;
uint8_t reserved1;

55
src/utilities/protocol_detector.cpp Normal file
View File

@@ -0,0 +1,55 @@
/*
* protocol_detector.cpp
*
* Created on: Jul 15, 2021 14:05
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include "ugv_sdk/utilities/protocol_detector.hpp"
#include "ugv_sdk/utilities/stopwatch.hpp"
namespace westonrobot {
void ProtocolDectctor::Connect(std::string can_name) {
can_ = std::make_shared<AsyncCAN>(can_name);
can_->SetReceiveCallback(
std::bind(&ProtocolDectctor::ParseCANFrame, this, std::placeholders::_1));
can_->StartListening();
}
void ProtocolDectctor::Connect(std::string uart_name, uint32_t baudrate) {}
ProtocolVersion ProtocolDectctor::DetectProtocolVersion(uint32_t timeout_sec) {
msg_v1_detected_ = false;
msg_v2_detected_ = false;
StopWatch sw;
Timer timer;
while (sw.stoc() < timeout_sec) {
timer.reset();
if (msg_v1_detected_ || msg_v2_detected_) break;
timer.sleep_until_ms(50);
}
// make sure only one version is detected
if (msg_v1_detected_ && msg_v2_detected_) return ProtocolVersion::UNKONWN;
if (msg_v1_detected_)
return ProtocolVersion::AGX_V1;
else if (msg_v2_detected_)
return ProtocolVersion::AGX_V2;
return ProtocolVersion::UNKONWN;
};
void ProtocolDectctor::ParseCANFrame(can_frame *rx_frame) {
// state feedback frame with id 0x151 is unique to V1 protocol
if (rx_frame->can_id == 0x151) {
msg_v1_detected_ = true;
}
// rc state frame with id 0x241 is unique to V2 protocol
else if (rx_frame->can_id == 0x241) {
msg_v2_detected_ = true;
}
}
} // namespace westonrobot