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

tracer protocol v2.0 works

Browse Source
This commit is contained in:
Ruixiang Du
2020-11-06 09:37:37 +08:00
parent 87cd3665aa
commit 3ca082d1f0
5 changed files with 58 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
ugv_sdk/apps/tracer_demo/tracer_demo.cpp
View File

@@ -28,55 +28,61 @@ int main(int argc, char **argv) {
tracer.Connect(device_name); tracer.Connect(device_name);
tracer.EnableCommandedMode(); tracer.EnableCommandedMode();
// tracer.DisableTimeout();
// light control // light control
std::cout << "Light: const off" << std::endl; std::cout << "Light: const off" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0, tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0,
TracerLightCmd::LightMode::CONST_OFF, 0}); TracerLightCmd::LightMode::CONST_OFF, 0});
sleep(5); // usleep(50000);
sleep(3);
std::cout << "Light: const on" << std::endl; std::cout << "Light: const on" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0, tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0,
TracerLightCmd::LightMode::CONST_ON, 0}); TracerLightCmd::LightMode::CONST_ON, 0});
// usleep(50000);
sleep(3); sleep(3);
std::cout << "Light: breath" << std::endl; std::cout << "Light: breath" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0, tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0,
TracerLightCmd::LightMode::BREATH, 0}); TracerLightCmd::LightMode::BREATH, 0});
sleep(3); // usleep(50000);
sleep(8);
std::cout << "Light: custom 90-80" << std::endl; std::cout << "Light: custom 90-80" << std::endl;
tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90, tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90,
TracerLightCmd::LightMode::CUSTOM, 80}); TracerLightCmd::LightMode::CUSTOM, 80});
// usleep(50000);
sleep(3); sleep(3);
std::cout << "Light: diabled cmd control" << std::endl; std::cout << "Light: diabled cmd control" << std::endl;
tracer.DisableLightCmdControl(); tracer.SetLightCommand(TracerLightCmd());
int count = 0; int count = 0;
while (true) { while (true) {
// // motion control // motion control
// if (count < 5) { if (count < 5) {
// std::cout << "Motor: 0.2, 0.0" << std::endl; std::cout << "Motor: 0.2, 0.0" << std::endl;
// tracer.SetMotionCommand(0.2, 0.0); tracer.SetMotionCommand(0.2, 0.0);
// } else if (count < 10) { } else if (count < 10) {
// std::cout << "Motor: 0.8, 0.3" << std::endl; std::cout << "Motor: 0.8, 0.3" << std::endl;
// tracer.SetMotionCommand(0.8, 0.3); tracer.SetMotionCommand(0.8, 0.3);
// } else if (count < 15) { } else if (count < 15) {
// std::cout << "Motor: 1.5, 0.5" << std::endl; std::cout << "Motor: 1.5, 0.5" << std::endl;
// tracer.SetMotionCommand(1.5, 0.5); tracer.SetMotionCommand(1.5, 0.5);
// } else if (count < 20) { } else if (count < 20) {
// std::cout << "Motor: 1.0, 0.3" << std::endl; std::cout << "Motor: 1.0, 0.3" << std::endl;
// tracer.SetMotionCommand(1.0, 0.3); tracer.SetMotionCommand(1.0, 0.3);
// } else if (count < 25) { } else if (count < 25) {
// std::cout << "Motor: 0.0, 0.0" << std::endl; std::cout << "Motor: 0.0, 0.0" << std::endl;
// tracer.SetMotionCommand(0.0, 0.0); tracer.SetMotionCommand(0.0, 0.0);
// } else if (count < 30) { } else if (count < 30) {
// std::cout << "Motor: -0.5, -0.3" << std::endl; std::cout << "Motor: -0.5, -0.3" << std::endl;
// tracer.SetMotionCommand(-0.5, -0.3); tracer.SetMotionCommand(-0.5, -0.3);
// } else if (count < 35) { } else if (count < 35) {
// std::cout << "Motor: -1.0, -0.5" << std::endl; std::cout << "Motor: -1.0, -0.5" << std::endl;
// tracer.SetMotionCommand(-1.0, -0.5); tracer.SetMotionCommand(-1.0, -0.5);
// } else if (count < 40) { } else if (count < 40) {
// std::cout << "Motor: 0.0, 0.0," << std::endl; std::cout << "Motor: 0.0, 0.0," << std::endl;
// tracer.SetMotionCommand(0.0, 0.0); tracer.SetMotionCommand(0.0, 0.0);
// } }
// tracer.SetMotionCommand(0.8, 0.8);
auto state = tracer.GetTracerState(); auto state = tracer.GetTracerState();
std::cout << "-------------------------------" << std::endl; std::cout << "-------------------------------" << std::endl;
@@ -89,6 +95,7 @@ int main(int argc, char **argv) {
<< state.angular_velocity << std::endl; << state.angular_velocity << std::endl;
std::cout << "-------------------------------" << std::endl; std::cout << "-------------------------------" << std::endl;
// usleep(20000);
sleep(1); sleep(1);
++count; ++count;
} }

5
ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp
View File

@@ -34,8 +34,7 @@ class TracerBase : public MobileBase {
void SetMotionCommand(double linear_vel, double angular_vel); void SetMotionCommand(double linear_vel, double angular_vel);
// light control // light control
void SetLightCommand(TracerLightCmd cmd); void SetLightCommand(const TracerLightCmd &cmd);
void DisableLightCmdControl();
private: private:
// cmd/status update related variables // cmd/status update related variables
@@ -53,7 +52,7 @@ class TracerBase : public MobileBase {
bool light_ctrl_requested_ = false; bool light_ctrl_requested_ = false;
void SendMotionCmd(uint8_t count); void SendMotionCmd(uint8_t count);
void SendLightCmd(uint8_t count); void SendLightCmd(const TracerLightCmd &cmd, uint8_t count);
void SendRobotCmd() override; void SendRobotCmd() override;
void ParseCANFrame(can_frame *rx_frame) override; void ParseCANFrame(can_frame *rx_frame) override;

4
ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp
View File

@@ -77,11 +77,13 @@ struct TracerLightCmd {
TracerLightCmd() = default; TracerLightCmd() = default;
TracerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, TracerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value) uint8_t r_value)
: front_mode(f_mode), : enable_ctrl(true),
front_mode(f_mode),
front_custom_value(f_value), front_custom_value(f_value),
rear_mode(r_mode), rear_mode(r_mode),
rear_custom_value(r_value) {} rear_custom_value(r_value) {}
bool enable_ctrl = false;
LightMode front_mode; LightMode front_mode;
uint8_t front_custom_value; uint8_t front_custom_value;
LightMode rear_mode; LightMode rear_mode;

13
ugv_sdk/src/agx_msg_parser.c
View File

@@ -108,8 +108,6 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_command_msg.raw, memcpy(tx_frame->data, msg->body.motion_command_msg.raw,
tx_frame->can_dlc); tx_frame->can_dlc);
// tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data,
// tx_frame->can_dlc);
break; break;
} }
case AgxMsgLightCommand: { case AgxMsgLightCommand: {
@@ -117,8 +115,6 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.light_command_msg.raw, memcpy(tx_frame->data, msg->body.light_command_msg.raw,
tx_frame->can_dlc); tx_frame->can_dlc);
// tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data,
// tx_frame->can_dlc);
break; break;
} }
case AgxMsgCtrlModeSelect: { case AgxMsgCtrlModeSelect: {
@@ -126,16 +122,12 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.ctrl_mode_select_msg.raw, memcpy(tx_frame->data, msg->body.ctrl_mode_select_msg.raw,
tx_frame->can_dlc); tx_frame->can_dlc);
// tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data,
// tx_frame->can_dlc);
break; break;
} }
case AgxMsgFaultByteReset: { case AgxMsgFaultByteReset: {
tx_frame->can_id = CAN_MSG_STATE_RESET_ID; tx_frame->can_id = CAN_MSG_STATE_RESET_ID;
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.state_reset_msg.raw, tx_frame->can_dlc); memcpy(tx_frame->data, msg->body.state_reset_msg.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data,
tx_frame->can_dlc);
break; break;
} }
// state feedback frame // state feedback frame
@@ -214,8 +206,9 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
default: default:
break; break;
} }
tx_frame->data[7] = // tx_frame->data[7] =
CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); // CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data,
// tx_frame->can_dlc);
} }
uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc) { uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc) {

45
ugv_sdk/src/tracer_base.cpp
View File

@@ -15,14 +15,9 @@
namespace westonrobot { namespace westonrobot {
void TracerBase::SendRobotCmd() { void TracerBase::SendRobotCmd() {
static uint8_t cmd_count = 0; static uint8_t cmd_count = 0;
static uint8_t light_cmd_count = 0; // EnableCommandedMode();
if (can_connected_) { if (can_connected_) {
SendMotionCmd(cmd_count++); SendMotionCmd(cmd_count++);
SendLightCmd(light_cmd_count++);
// if (light_ctrl_requested_)
// SendLightCmd(light_cmd_count++);
// else
// std::cout << "not updating light cmd" << std::endl;
} }
} }
@@ -46,9 +41,9 @@ void TracerBase::SendMotionCmd(uint8_t count) {
motion_cmd_mutex_.lock(); motion_cmd_mutex_.lock();
int16_t linear_cmd = int16_t linear_cmd =
static_cast<int16_t>(current_motion_cmd_.linear_velocity) * 1000; static_cast<int16_t>(current_motion_cmd_.linear_velocity * 1000);
int16_t angular_cmd = int16_t angular_cmd =
static_cast<int16_t>(current_motion_cmd_.angular_velocity) * 1000; static_cast<int16_t>(current_motion_cmd_.angular_velocity * 1000);
motion_cmd_mutex_.unlock(); motion_cmd_mutex_.unlock();
// SendControlCmd(); // SendControlCmd();
@@ -67,28 +62,24 @@ void TracerBase::SendMotionCmd(uint8_t count) {
can_if_->SendFrame(m_frame); can_if_->SendFrame(m_frame);
} }
void TracerBase::SendLightCmd(uint8_t count) { void TracerBase::SendLightCmd(const TracerLightCmd &lcmd, uint8_t count) {
AgxMessage l_msg; AgxMessage l_msg;
l_msg.type = AgxMsgLightCommand; l_msg.type = AgxMsgLightCommand;
memset(l_msg.body.light_command_msg.raw, 0, 8); memset(l_msg.body.light_command_msg.raw, 0, 8);
light_cmd_mutex_.lock(); if (lcmd.enable_ctrl) {
if (light_ctrl_enabled_) {
l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE; l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE;
l_msg.body.light_command_msg.cmd.front_light_mode = l_msg.body.light_command_msg.cmd.front_light_mode =
static_cast<uint8_t>(current_light_cmd_.front_mode); static_cast<uint8_t>(lcmd.front_mode);
l_msg.body.light_command_msg.cmd.front_light_custom = l_msg.body.light_command_msg.cmd.front_light_custom =
current_light_cmd_.front_custom_value; lcmd.front_custom_value;
l_msg.body.light_command_msg.cmd.rear_light_mode = l_msg.body.light_command_msg.cmd.rear_light_mode =
static_cast<uint8_t>(current_light_cmd_.rear_mode); static_cast<uint8_t>(lcmd.rear_mode);
l_msg.body.light_command_msg.cmd.rear_light_custom = l_msg.body.light_command_msg.cmd.rear_light_custom = lcmd.rear_custom_value;
current_light_cmd_.rear_custom_value;
} else { } else {
l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE; l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE;
} }
light_ctrl_requested_ = false;
light_cmd_mutex_.unlock();
l_msg.body.light_command_msg.cmd.count = count; l_msg.body.light_command_msg.cmd.count = count;
@@ -96,6 +87,8 @@ void TracerBase::SendLightCmd(uint8_t count) {
can_frame l_frame; can_frame l_frame;
EncodeCanFrame(&l_msg, &l_frame); EncodeCanFrame(&l_msg, &l_frame);
can_if_->SendFrame(l_frame); can_if_->SendFrame(l_frame);
std::cout << "light cmd sent <---------------------" << std::endl;
} }
TracerState TracerBase::GetTracerState() { TracerState TracerBase::GetTracerState() {
@@ -123,19 +116,9 @@ void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) {
FeedCmdTimeoutWatchdog(); FeedCmdTimeoutWatchdog();
} }
void TracerBase::SetLightCommand(TracerLightCmd cmd) { void TracerBase::SetLightCommand(const TracerLightCmd &cmd) {
if (!cmd_thread_started_) StartCmdThread(); static uint8_t light_cmd_count = 0;
SendLightCmd(cmd, light_cmd_count++);
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) { void TracerBase::ParseCANFrame(can_frame *rx_frame) {