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 parser for scout

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-12 16:30:28 +08:00
parent 4ebef2e906
commit 10d9fc734e
12 changed files with 225 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@@ -81,11 +81,10 @@ add_library(${PROJECT_NAME}
########################
## protocol v2 support
src/protocol_v2/agilex_msg_parser_v2.c
src/protocol_v2/parser_impl/protocol_v2_parser.cpp
src/protocol_v2/protocol_v2_parser.cpp
########################
## legacy protocol v1 support (transparent to user)
src/protocol_v1/agilex_msg_parser_v1.c
src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp
)
target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads)
target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES)

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

@@ -331,7 +331,7 @@ typedef struct {
// V1-only messages
MotionCommandMessageV1 v1_motion_command_msg;
ValueSetCommandMessageV1 v1_value_set_command_msg;
ActuatorStateMessageV1 v1_actuator_stage_msg;
ActuatorStateMessageV1 v1_actuator_state_msg;
} body;
} AgxMessage;

0
src/protocol_v1/agilex_msg_parser_v1.h → include/ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h
View File

0
src/protocol_v1/agilex_protocol_v1.h → include/ugv_sdk/details/protocol_v1/agilex_protocol_v1.h
View File

66
include/ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp Normal file
View File

@@ -0,0 +1,66 @@
/*
* scout_protocol_v1_parser.hpp
*
* Created on: Jul 08, 2021 22:30
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef SCOUT_PROTOCOL_V1_PARSER_HPP
#define SCOUT_PROTOCOL_V1_PARSER_HPP
#include "ugv_sdk/details/interface/parser_interface.hpp"
#include "ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h"
#include "ugv_sdk/details/protocol_v1/robot_limits.hpp"
namespace westonrobot {
template <typename RobotLimitsType>
class DiffDriveModelV1Parser : public ParserInterface {
public:
bool DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) override {
return DecodeCanFrameV1(rx_frame, msg);
}
void EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) override {
AgxMessage msg_v1 = *msg;
if (msg->type == AgxMsgMotionCommand) {
float linear = msg->body.motion_command_msg.linear_velocity;
float angular = msg->body.motion_command_msg.angular_velocity;
if (linear > ScoutV2Limits::max_linear_velocity)
linear = ScoutV2Limits::max_linear_velocity;
else if (linear < ScoutV2Limits::min_linear_velocity)
linear = ScoutV2Limits::min_linear_velocity;
if (angular > ScoutV2Limits::max_angular_velocity)
angular = ScoutV2Limits::max_angular_velocity;
else if (angular < ScoutV2Limits::min_angular_velocity)
angular = ScoutV2Limits::min_angular_velocity;
msg_v1.body.motion_command_msg.linear_velocity =
linear / ScoutV2Limits::max_linear_velocity * 100.0;
msg_v1.body.motion_command_msg.angular_velocity = static_cast<int8_t>(
angular / ScoutV2Limits::max_angular_velocity * 100.0);
}
EncodeCanFrameV1(&msg_v1, tx_frame);
}
uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override {
return CalcCanFrameChecksumV1(id, data, dlc);
}
// UART support
bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override {}
void EncodeMessage(const AgxMessage *msg, uint8_t *buf,
uint8_t *len) override {}
uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override {}
};
using ScoutProtocolV1Parser = DiffDriveModelV1Parser<ScoutV2Limits>;
using ScoutMiniProtocolV1Parser = DiffDriveModelV1Parser<ScoutV2Limits>;
} // namespace westonrobot
#endif /* SCOUT_PROTOCOL_V1_PARSER_HPP */

26
include/ugv_sdk/details/protocol_v1/hunter_protocol_v1_parser.hpp
View File

@@ -1,26 +0,0 @@
/*
* hunter_protocol_v1_parser.hpp
*
* Created on: Jul 09, 2021 22:20
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef HUNTER_PROTOCOL_V1_PARSER_HPP
#define HUNTER_PROTOCOL_V1_PARSER_HPP
#include "ugv_sdk/details/interface/parser_interface.hpp"
namespace westonrobot {
class HunterProtocolV1Parser : public ParserInterface {
public:
bool DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) override;
void EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) override;
uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override;
};
} // namespace westonrobot
#endif /* HUNTER_PROTOCOL_V1_PARSER_HPP */

29
include/ugv_sdk/details/protocol_v1/robot_limits.hpp Normal file
View File

@@ -0,0 +1,29 @@
/*
* robot_limits.hpp
*
* Created on: Jul 12, 2021 14:01
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#ifndef ROBOT_LIMITS_HPP
#define ROBOT_LIMITS_HPP
namespace westonrobot {
struct ScoutV2Limits {
static constexpr double max_linear_velocity = 1.5; // 1.5 m/s
static constexpr double min_linear_velocity = -max_linear_velocity;
static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s
static constexpr double min_angular_velocity = -max_angular_velocity;
};
struct ScoutMiniLimits {
static constexpr double max_linear_velocity = 3.0; // 3.0 m/s
static constexpr double min_linear_velocity = -max_linear_velocity;
static constexpr double max_angular_velocity = 2.5235; // 2.5235 rad/s
static constexpr double min_angular_velocity = -max_angular_velocity;
};
} // namespace westonrobot
#endif /* ROBOT_LIMITS_HPP */

32
include/ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp
View File

@@ -1,32 +0,0 @@
/*
* scout_protocol_v1_parser.hpp
*
* Created on: Jul 08, 2021 22:30
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef SCOUT_PROTOCOL_V1_PARSER_HPP
#define SCOUT_PROTOCOL_V1_PARSER_HPP
#include "ugv_sdk/details/interface/parser_interface.hpp"
namespace westonrobot {
class ScoutProtocolV1Parser : public ParserInterface {
public:
bool DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) override;
void EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) override;
uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override;
// UART support
bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override;
void EncodeMessage(const AgxMessage *msg, uint8_t *buf,
uint8_t *len) override;
uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override;
};
} // namespace westonrobot
#endif /* SCOUT_PROTOCOL_V1_PARSER_HPP */

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

@@ -84,10 +84,6 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
state.light_state = status_msg.body.light_state_msg;
break;
}
// case AgxMsgRcState: {
// state.rc_state = status_msg.body.rc_state_msg;
// break;
// }
case AgxMsgActuatorHSState: {
// std::cout << "actuator hs feedback received" << std::endl;
state
@@ -102,6 +98,10 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
status_msg.body.actuator_ls_state_msg;
break;
}
// case AgxMsgRcState: {
// state.rc_state = status_msg.body.rc_state_msg;
// break;
// }
/* sensor feedback */
// case AgxMsgOdometry: {
// // std::cout << "Odometer msg feedback received" << std::endl;
@@ -114,7 +114,7 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
};
} // namespace westonrobot
#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp"
#include "ugv_sdk/details/protocol_v1/diffdrive_model_v1_parser.hpp"
#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
namespace westonrobot {

128
src/protocol_v1/agilex_msg_parser_v1.c
View File

@@ -7,12 +7,130 @@
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#include "agilex_protocol_v1.h"
#include "protocol_v1/agilex_msg_parser_v1.h"
#include "ugv_sdk/details/protocol_v1/agilex_protocol_v1.h"
#include "ugv_sdk/details/protocol_v1//agilex_msg_parser_v1.h"
#include "stdio.h"
#include "string.h"
bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {}
void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {}
uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc) {}
bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
switch (rx_frame->can_id) {
case CAN_MSG_SYSTEM_STATE_ID: {
msg->type = AgxMsgSystemState;
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
// memcpy(msg->body.system_state_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTION_STATE_ID: {
msg->type = AgxMsgMotionState;
// msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
// memcpy(msg->body.motion_state_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_LIGHT_STATE_ID: {
msg->type = AgxMsgLightState;
// msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
// memcpy(msg->body.light_state_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_ACTUATOR1_STATE_ID: {
msg->type = AgxMsgActuatorStateV1;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
// msg->body.v1_actuator_state_msg.motor_id = SCOUT_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_ACTUATOR2_STATE_ID: {
msg->type = AgxMsgActuatorStateV1;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
// msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR2_ID;
// memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_ACTUATOR3_STATE_ID: {
msg->type = AgxMsgActuatorStateV1;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
// msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR3_ID;
// memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_ACTUATOR4_STATE_ID: {
msg->type = AgxMsgActuatorStateV1;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
// msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR4_ID;
// memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
return true;
}
void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
switch (msg->type) {
case AgxMsgMotionCommandV1: {
static uint8_t count = 0;
tx_frame->can_id = CAN_MSG_MOTION_COMMAND_ID;
tx_frame->can_dlc = 8;
tx_frame->data[0] = CTRL_MODE_CMD_CAN;
tx_frame->data[1] = ERROR_CLR_NONE;
tx_frame->data[2] =
(int8_t)(msg->body.motion_command_msg.linear_velocity * 100);
tx_frame->data[3] =
(int8_t)(msg->body.motion_command_msg.angular_velocity * 100);
tx_frame->data[4] = 0;
tx_frame->data[5] = 0;
tx_frame->data[6] = count++;
tx_frame->data[7] = CalcCanFrameChecksumV1(
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
break;
}
case AgxMsgValueSetCommandV1: {
static uint8_t count = 0;
tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID;
tx_frame->can_dlc = 8;
LightCommandFrame frame;
if (msg->body.light_command_msg.enable_cmd_ctrl) {
frame.enable_cmd_ctrl = LIGHT_ENABLE_CMD_CTRL;
frame.front_mode = msg->body.light_command_msg.front_light.mode;
frame.front_custom =
msg->body.light_command_msg.front_light.custom_value;
frame.rear_mode = msg->body.light_command_msg.rear_light.mode;
frame.rear_custom = msg->body.light_command_msg.rear_light.custom_value;
} else {
frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL;
frame.front_mode = 0;
frame.front_custom = 0;
frame.rear_mode = 0;
frame.rear_custom = 0;
}
frame.reserved0 = 0;
frame.count = count++;
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
tx_frame->data[7] = CalcCanFrameChecksumV1(
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
break;
}
case AgxMsgLightCommand: {
break;
}
default:
break;
}
}
uint8_t CalcCanFrameChecksumV1(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;
}

111
src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp
View File

@@ -1,111 +0,0 @@
/*
* scout_protocol_v1_parser.cpp
*
* Created on: Jul 08, 2021 22:43
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp"
namespace westonrobot {
// CAN support
bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) {
// ScoutMessage scout_msg;
// if ScoutMessage found, convert to AgxMessage
// if (DecodeScoutMsgFromCAN(rx_frame, &scout_msg)) {
// switch (scout_msg.type) {
// case ScoutMotionStatusMsg: {
// break;
// }
// case ScoutLightStatusMsg: {
// break;
// }
// case ScoutSystemStatusMsg: {
// break;
// }
// case ScoutMotorDriverStatusMsg: {
// break;
// }
// }
// }
return false;
}
void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) {
// ScoutMessage scout_msg;
// convert to ScoutMessage, then encode to can frame
// switch (msg->type) {
// case AgxMsgMotionCommand: {
// // scout_msg.type = ScoutMotionControlMsg;
// // scout_msg.body.motion_control_msg.data.cmd.control_mode =
// // CTRL_MODE_COMMANDED;
// // scout_msg.body.motion_control_msg.data.cmd.control_mode =
// // CTRL_MODE_CMD_CAN;
// // scout_msg.body.motion_control_msg.data.cmd.fault_clear_flag =
// 0x00;
// /*
// std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
// current_motion_cmd_.linear_velocity = static_cast<int8_t>(
// linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0);
// current_motion_cmd_.angular_velocity = static_cast<int8_t>(
// angular_vel / ScoutCmdLimits::max_angular_velocity * 100.0);
// */
// // scout_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd =
// // current_motion_cmd_.linear_velocity;
// // scout_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd =
// // current_motion_cmd_.angular_velocity;
// // scout_msg.body.motion_control_msg.data.cmd.reserved0 = 0;
// // scout_msg.body.motion_control_msg.data.cmd.reserved1 = 0;
// // scout_msg.body.motion_control_msg.data.cmd.count = count;
// // scout_msg.body.motion_control_msg.data.cmd.checksum =
// // CalculateChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID,
// // scout_msg.body.motion_control_msg.data.raw,
// 8);
// break;
// }
// case AgxMsgLightCommand: {
// scout_msg.body.light_control_msg.data.cmd.light_ctrl_enable =
// LIGHT_ENABLE_CTRL;
// // scout_msg.body.light_control_msg.data.cmd.front_mode =
// // static_cast<uint8_t>(current_light_cmd_.front_mode);
// // scout_msg.body.light_control_msg.data.cmd.front_custom =
// // current_light_cmd_.front_custom_value;
// // scout_msg.body.light_control_msg.data.cmd.rear_mode =
// // static_cast<uint8_t>(current_light_cmd_.rear_mode);
// // scout_msg.body.light_control_msg.data.cmd.rear_custom =
// // current_light_cmd_.rear_custom_value;
// // scout_msg.body.light_control_msg.data.cmd.reserved0 = 0;
// // scout_msg.body.light_control_msg.data.cmd.count = count;
// scout_msg.body.light_control_msg.data.cmd.checksum =
// CalculateChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID,
// scout_msg.body.light_control_msg.data.raw, 8);
// break;
// }
// }
}
uint8_t ScoutProtocolV1Parser::CalculateChecksum(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;
}
// UART support
bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc,
AgxMessage *msg) {}
void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf,
uint8_t *len) {}
uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {}
} // namespace westonrobot

0
src/protocol_v2/parser_impl/protocol_v2_parser.cpp → src/protocol_v2/protocol_v2_parser.cpp
View File