mirror of
https://github.com/westonrobot/ugv_sdk
synced 2023-04-08 06:32:14 +08:00
232 lines
8.7 KiB
C++
232 lines
8.7 KiB
C++
#include "ugv_sdk/scout/scout_base.hpp"
|
|
|
|
#include <stdio.h>
|
|
#include <string>
|
|
#include <cstring>
|
|
#include <iostream>
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <chrono>
|
|
#include <cstdint>
|
|
#include <ratio>
|
|
#include <thread>
|
|
|
|
#include "stopwatch.hpp"
|
|
|
|
namespace westonrobot {
|
|
void ScoutBase::SendRobotCmd() {
|
|
static uint8_t cmd_count = 0;
|
|
// EnableCommandedMode();
|
|
if (can_connected_) {
|
|
SendMotionCmd(cmd_count++);
|
|
}
|
|
}
|
|
|
|
void ScoutBase::EnableCommandedMode() {
|
|
AgxMessage c_msg;
|
|
c_msg.type = AgxMsgCtrlModeSelect;
|
|
memset(c_msg.body.ctrl_mode_select_msg.raw, 0, 8);
|
|
c_msg.body.ctrl_mode_select_msg.cmd.control_mode = CTRL_MODE_CMD_CAN;
|
|
|
|
// send to can bus
|
|
can_frame c_frame;
|
|
EncodeCanFrame(&c_msg, &c_frame);
|
|
can_if_->SendFrame(c_frame);
|
|
}
|
|
|
|
void ScoutBase::SendMotionCmd(uint8_t count) {
|
|
// motion control message
|
|
AgxMessage m_msg;
|
|
m_msg.type = AgxMsgMotionCommand;
|
|
memset(m_msg.body.motion_command_msg.raw, 0, 8);
|
|
|
|
motion_cmd_mutex_.lock();
|
|
int16_t linear_cmd =
|
|
static_cast<int16_t>(current_motion_cmd_.linear_velocity * 1000);
|
|
int16_t angular_cmd =
|
|
static_cast<int16_t>(current_motion_cmd_.angular_velocity * 1000);
|
|
motion_cmd_mutex_.unlock();
|
|
|
|
// SendControlCmd();
|
|
m_msg.body.motion_command_msg.cmd.linear_velocity.high_byte =
|
|
(static_cast<uint16_t>(linear_cmd) >> 8) & 0x00ff;
|
|
m_msg.body.motion_command_msg.cmd.linear_velocity.low_byte =
|
|
(static_cast<uint16_t>(linear_cmd) >> 0) & 0x00ff;
|
|
m_msg.body.motion_command_msg.cmd.angular_velocity.high_byte =
|
|
(static_cast<uint16_t>(angular_cmd) >> 8) & 0x00ff;
|
|
m_msg.body.motion_command_msg.cmd.angular_velocity.low_byte =
|
|
(static_cast<uint16_t>(angular_cmd) >> 0) & 0x00ff;
|
|
|
|
// send to can bus
|
|
can_frame m_frame;
|
|
EncodeCanFrame(&m_msg, &m_frame);
|
|
can_if_->SendFrame(m_frame);
|
|
}
|
|
|
|
void ScoutBase::SendLightCmd(const ScoutLightCmd &lcmd, uint8_t count) {
|
|
AgxMessage l_msg;
|
|
l_msg.type = AgxMsgLightCommand;
|
|
memset(l_msg.body.light_command_msg.raw, 0, 8);
|
|
|
|
if (lcmd.enable_ctrl) {
|
|
l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE;
|
|
|
|
l_msg.body.light_command_msg.cmd.front_light_mode =
|
|
static_cast<uint8_t>(lcmd.front_mode);
|
|
l_msg.body.light_command_msg.cmd.front_light_custom =
|
|
lcmd.front_custom_value;
|
|
l_msg.body.light_command_msg.cmd.rear_light_mode =
|
|
static_cast<uint8_t>(lcmd.rear_mode);
|
|
l_msg.body.light_command_msg.cmd.rear_light_custom = lcmd.rear_custom_value;
|
|
} else {
|
|
l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE;
|
|
}
|
|
|
|
l_msg.body.light_command_msg.cmd.count = count;
|
|
|
|
// send to can bus
|
|
can_frame l_frame;
|
|
EncodeCanFrame(&l_msg, &l_frame);
|
|
can_if_->SendFrame(l_frame);
|
|
}
|
|
|
|
ScoutState ScoutBase::GetScoutState() {
|
|
std::lock_guard<std::mutex> guard(scout_state_mutex_);
|
|
return scout_state_;
|
|
}
|
|
|
|
void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) {
|
|
// make sure cmd thread is started before attempting to send commands
|
|
if (!cmd_thread_started_) StartCmdThread();
|
|
|
|
if (linear_vel < ScoutMotionCmd::min_linear_velocity)
|
|
linear_vel = ScoutMotionCmd::min_linear_velocity;
|
|
if (linear_vel > ScoutMotionCmd::max_linear_velocity)
|
|
linear_vel = ScoutMotionCmd::max_linear_velocity;
|
|
if (angular_vel < ScoutMotionCmd::min_angular_velocity)
|
|
angular_vel = ScoutMotionCmd::min_angular_velocity;
|
|
if (angular_vel > ScoutMotionCmd::max_angular_velocity)
|
|
angular_vel = ScoutMotionCmd::max_angular_velocity;
|
|
|
|
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
|
|
current_motion_cmd_.linear_velocity = linear_vel;
|
|
current_motion_cmd_.angular_velocity = angular_vel;
|
|
|
|
FeedCmdTimeoutWatchdog();
|
|
}
|
|
|
|
void ScoutBase::SetLightCommand(const ScoutLightCmd &cmd) {
|
|
static uint8_t light_cmd_count = 0;
|
|
SendLightCmd(cmd, light_cmd_count++);
|
|
}
|
|
|
|
void ScoutBase::ParseCANFrame(can_frame *rx_frame) {
|
|
AgxMessage status_msg;
|
|
DecodeCanFrame(rx_frame, &status_msg);
|
|
NewStatusMsgReceivedCallback(status_msg);
|
|
}
|
|
|
|
void ScoutBase::NewStatusMsgReceivedCallback(const AgxMessage &msg) {
|
|
// std::cout << "new status msg received" << std::endl;
|
|
std::lock_guard<std::mutex> guard(scout_state_mutex_);
|
|
UpdateScoutState(msg, scout_state_);
|
|
}
|
|
|
|
void ScoutBase::UpdateScoutState(const AgxMessage &status_msg,
|
|
ScoutState &state) {
|
|
switch (status_msg.type) {
|
|
case AgxMsgSystemState: {
|
|
// std::cout << "system status feedback received" << std::endl;
|
|
const SystemStateMessage &msg = status_msg.body.system_state_msg;
|
|
state.control_mode = msg.state.control_mode;
|
|
state.base_state = msg.state.vehicle_state;
|
|
state.battery_voltage =
|
|
(static_cast<uint16_t>(msg.state.battery_voltage.low_byte) |
|
|
static_cast<uint16_t>(msg.state.battery_voltage.high_byte) << 8) /
|
|
10.0;
|
|
state.fault_code = msg.state.fault_code;
|
|
break;
|
|
}
|
|
case AgxMsgMotionState: {
|
|
// std::cout << "motion control feedback received" << std::endl;
|
|
const MotionStateMessage &msg = status_msg.body.motion_state_msg;
|
|
state.linear_velocity =
|
|
static_cast<int16_t>(
|
|
static_cast<uint16_t>(msg.state.linear_velocity.low_byte) |
|
|
static_cast<uint16_t>(msg.state.linear_velocity.high_byte) << 8) /
|
|
1000.0;
|
|
state.angular_velocity =
|
|
static_cast<int16_t>(
|
|
static_cast<uint16_t>(msg.state.angular_velocity.low_byte) |
|
|
static_cast<uint16_t>(msg.state.angular_velocity.high_byte)
|
|
<< 8) /
|
|
1000.0;
|
|
break;
|
|
}
|
|
case AgxMsgLightState: {
|
|
// std::cout << "light control feedback received" << std::endl;
|
|
const LightStateMessage &msg = status_msg.body.light_state_msg;
|
|
if (msg.state.light_ctrl_enabled == LIGHT_CTRL_DISABLE)
|
|
state.light_control_enabled = false;
|
|
else
|
|
state.light_control_enabled = true;
|
|
state.front_light_state.mode = msg.state.front_light_mode;
|
|
state.front_light_state.custom_value = msg.state.front_light_custom;
|
|
state.rear_light_state.mode = msg.state.rear_light_mode;
|
|
state.rear_light_state.custom_value = msg.state.rear_light_custom;
|
|
break;
|
|
}
|
|
case AgxMsgActuatorHSState: {
|
|
// std::cout << "actuator hs feedback received" << std::endl;
|
|
const ActuatorHSStateMessage &msg = status_msg.body.actuator_hs_state_msg;
|
|
state.actuator_states[msg.motor_id].motor_current =
|
|
(static_cast<uint16_t>(msg.data.state.current.low_byte) |
|
|
static_cast<uint16_t>(msg.data.state.current.high_byte) << 8) /
|
|
10.0;
|
|
state.actuator_states[msg.motor_id].motor_rpm = static_cast<int16_t>(
|
|
static_cast<uint16_t>(msg.data.state.rpm.low_byte) |
|
|
static_cast<uint16_t>(msg.data.state.rpm.high_byte) << 8);
|
|
state.actuator_states[msg.motor_id].motor_pulses = static_cast<int16_t>(
|
|
static_cast<uint16_t>(msg.data.state.pulse_count.low_byte) |
|
|
static_cast<uint16_t>(msg.data.state.pulse_count.high_byte) << 8);
|
|
break;
|
|
}
|
|
case AgxMsgActuatorLSState: {
|
|
// std::cout << "actuator ls feedback received" << std::endl;
|
|
const ActuatorLSStateMessage &msg = status_msg.body.actuator_ls_state_msg;
|
|
for (int i = 0; i < 2; ++i) {
|
|
state.actuator_states[msg.motor_id].driver_voltage =
|
|
(static_cast<uint16_t>(msg.data.state.driver_voltage.low_byte) |
|
|
static_cast<uint16_t>(msg.data.state.driver_voltage.high_byte)
|
|
<< 8) /
|
|
10.0;
|
|
state.actuator_states[msg.motor_id]
|
|
.driver_temperature = static_cast<int16_t>(
|
|
static_cast<uint16_t>(msg.data.state.driver_temperature.low_byte) |
|
|
static_cast<uint16_t>(msg.data.state.driver_temperature.high_byte)
|
|
<< 8);
|
|
state.actuator_states[msg.motor_id].motor_temperature =
|
|
msg.data.state.motor_temperature;
|
|
state.actuator_states[msg.motor_id].driver_state =
|
|
msg.data.state.driver_state;
|
|
}
|
|
break;
|
|
}
|
|
case AgxMsgOdometry: {
|
|
// std::cout << "Odometer msg feedback received" << std::endl;
|
|
const OdometryMessage &msg = status_msg.body.odometry_msg;
|
|
state.right_odometry = static_cast<int32_t>(
|
|
(static_cast<uint32_t>(msg.state.right_wheel.lsb)) |
|
|
(static_cast<uint32_t>(msg.state.right_wheel.low_byte) << 8) |
|
|
(static_cast<uint32_t>(msg.state.right_wheel.high_byte) << 16) |
|
|
(static_cast<uint32_t>(msg.state.right_wheel.msb) << 24));
|
|
state.left_odometry = static_cast<int32_t>(
|
|
(static_cast<uint32_t>(msg.state.left_wheel.lsb)) |
|
|
(static_cast<uint32_t>(msg.state.left_wheel.low_byte) << 8) |
|
|
(static_cast<uint32_t>(msg.state.left_wheel.high_byte) << 16) |
|
|
(static_cast<uint32_t>(msg.state.left_wheel.msb) << 24));
|
|
}
|
|
}
|
|
}
|
|
} // namespace westonrobot
|