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

saved work

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-08 16:22:41 +08:00
parent 2a67bbceb4
commit d2f210e6c1
56 changed files with 777 additions and 2259 deletions
Download Patch File Download Diff File Expand all files Collapse all files

170
src/agilex_base.cpp
View File

@@ -1,170 +0,0 @@
/*
* agilex_base.cpp
*
* Created on: Dec 22, 2020 17:20
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "ugv_sdk/agilex_base.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h"
#include "stopwatch.hpp"
namespace westonrobot {
AgilexBase::~AgilexBase() {
// release resource if occupied
Disconnect();
// joint cmd thread
if (cmd_thread_.joinable()) cmd_thread_.join();
}
void AgilexBase::Connect(std::string dev_name, CANFrameRxCallback cb) {
can_ = std::make_shared<AsyncCAN>(dev_name);
can_->SetReceiveCallback(cb);
can_->StartListening();
can_connected_ = true;
}
void AgilexBase::Disconnect() {
if (can_connected_) can_->StopService();
}
void AgilexBase::Terminate() {
keep_running_ = false;
std::terminate();
}
void AgilexBase::SetCmdThreadPeriodMs(int32_t period_ms) {
cmd_thread_period_ms_ = period_ms;
};
void AgilexBase::EnableCmdTimeout(uint32_t timeout_ms) {
enable_timeout_ = true;
timeout_ms_ = timeout_ms;
}
void AgilexBase::StartCmdThread() {
keep_running_ = true;
cmd_thread_ = std::thread(
std::bind(&AgilexBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void AgilexBase::ControlLoop(int32_t period_ms) {
uint32_t timeout_iter_num;
if (enable_timeout_) {
if (timeout_ms_ < period_ms) timeout_ms_ = period_ms;
timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms);
// std::cout << "Timeout iteration number: " << timeout_iter_num <<
// std::endl;
}
Timer tm;
while (keep_running_) {
tm.reset();
if (enable_timeout_) {
if (watchdog_counter_ < timeout_iter_num) {
SendRobotCmd();
++watchdog_counter_;
}
// else {
// std::cout << "Warning: cmd timeout, no cmd sent to robot" <<
// std::endl;
// }
} else {
SendRobotCmd();
}
tm.sleep_until_ms(period_ms);
}
}
void AgilexBase::EnableCommandedMode() {
// construct message
AgxMessage msg;
msg.type = AgxMsgControlModeConfig;
msg.body.control_mode_config_msg.mode = CONTROL_MODE_CAN;
// encode msg to can frame and send to bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
void AgilexBase::SetMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity,
double steering_angle) {
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread();
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = linear_vel;
current_motion_cmd_.angular_velocity = angular_vel;
current_motion_cmd_.lateral_velocity = lateral_velocity;
current_motion_cmd_.steering_angle = steering_angle;
FeedCmdTimeoutWatchdog();
}
void AgilexBase::SendRobotCmd() {
if (can_connected_) {
// motion control message
AgxMessage msg;
msg.type = AgxMsgMotionCommand;
motion_cmd_mutex_.lock();
msg.body.motion_command_msg = current_motion_cmd_;
motion_cmd_mutex_.unlock();
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
}
void AgilexBase::SendLightCommand(LightMode front_mode,
uint8_t front_custom_value,
LightMode rear_mode,
uint8_t rear_custom_value) {
AgxMessage msg;
msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = true;
msg.body.light_command_msg.front_light.mode = front_mode;
msg.body.light_command_msg.front_light.custom_value = front_custom_value;
msg.body.light_command_msg.rear_light.mode = rear_mode;
msg.body.light_command_msg.rear_light.custom_value = rear_custom_value;
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
void AgilexBase::DisableLightControl() {
AgxMessage msg;
msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = false;
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
void AgilexBase::SetMotionMode(uint8_t mode) {
AgxMessage msg;
msg.type = AgxMsgSetMotionMode;
msg.body.motion_mode_msg.motion_mode = mode;
// send to can bus
can_frame frame;
EncodeCanFrame(&msg, &frame);
can_->SendFrame(frame);
}
} // namespace westonrobot

14
src/mobile_base/scout_robot.cpp Normal file
View File

@@ -0,0 +1,14 @@
/*
* scout_robot.cpp
*
* Created on: Jul 08, 2021 11:13
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include "ugv_sdk/mobile_base/scout_robot.hpp"
namespace westonrobot {
ScoutRobot::ScoutRobot(AgilexProtocol protocol) {}
} // namespace westonrobot

2
src/legacy/scout_can_parser_v1.c → src/protocol_v1/scout_can_parser_v1.c
View File

@@ -7,7 +7,7 @@
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/legacy/scout_can_parser_v1.h"
#include "ugv_sdk/protocol_v1/scout_can_parser_v1.h"
#include "string.h"

2
src/agx_msg_parser.c → src/protocol_v2/agx_msg_parser.c
View File

@@ -8,7 +8,7 @@
*/
#include "agx_protocol_v2.h"
#include "ugv_sdk/details/agilex_msg_parser.h"
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
#include "stdio.h"
#include "string.h"

0
src/agx_protocol_v2.h → src/protocol_v2/agx_protocol_v2.h
View File

29
src/protocol_v2/protocol_v2_parser.cpp Normal file
View File

@@ -0,0 +1,29 @@
/*
* protocol_v2_parser.cpp
*
* Created on: Jul 08, 2021 14:53
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp"
namespace westonrobot {
bool ProtocolV2Parser::DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) {
return DecodeCanFrame(rx_frame, msg);
}
void ProtocolV2Parser::EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) {
EncodeCanFrame(msg, tx_frame);
}
uint8_t ProtocolV2Parser::CalculateChecksum(uint16_t id, uint8_t *data,
uint8_t dlc) {
return CalcCanFrameChecksum(id, data, dlc);
}
} // namespace westonrobot

6
src/ranger_base.cpp → src/protocol_v2/ranger_base.cpp
View File

@@ -7,7 +7,7 @@
* Copyright : AgileX Robotics
**/
#include "ugv_sdk/ranger_base.hpp"
#include "ugv_sdk/protocol_v2/ranger_base.hpp"
#include <algorithm>
#include <array>
@@ -21,11 +21,11 @@
#include "stopwatch.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h"
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot {
void RangerBase::Connect(std::string dev_name) {
AgilexBase::Connect(dev_name, std::bind(&RangerBase::ParseCANFrame, this,
AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame, this,
std::placeholders::_1));
}

35
src/scout_base.cpp → src/protocol_v2/scout_base.cpp
View File

@@ -1,4 +1,13 @@
#include "ugv_sdk/scout_base.hpp"
/*
* scout_base.cpp
*
* Created on: Jul 08, 2021 12:07
* Description:
*
* Copyright (c) 2021 Weston Robot Pte. Ltd.
*/
#include "ugv_sdk/protocol_v2/scout_base_v2.hpp"
#include <string>
#include <cstring>
@@ -10,28 +19,28 @@
#include <ratio>
#include <thread>
#include "stopwatch.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h"
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot {
void ScoutBase::Connect(std::string dev_name) {
AgilexBase::Connect(dev_name, std::bind(&ScoutBase::ParseCANFrame, this,
std::placeholders::_1));
AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBase::ParseCANFrame, this,
std::placeholders::_1));
}
void ScoutBase::Connect(std::string uart_name, uint32_t baudrate) {
// TODO
}
void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) {
AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
}
void ScoutBase::SetLightCommand(const ScoutLightCmd &cmd) {
if (cmd.cmd_ctrl_allowed) {
AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
LightMode::CONST_OFF, 0);
}
void ScoutBase::SetLightCommand(LightMode f_mode, uint8_t f_value,
LightMode r_mode, uint8_t r_value) {
AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value);
}
ScoutState ScoutBase::GetScoutState() {
ScoutState ScoutBase::GetRobotState() {
std::lock_guard<std::mutex> guard(state_mutex_);
return scout_state_;
}

12
src/tracer_base.cpp → src/protocol_v2/tracer_base.cpp
View File

@@ -1,4 +1,4 @@
#include "ugv_sdk/tracer_base.hpp"
#include "ugv_sdk/protocol_v2/tracer_base.hpp"
#include <string>
#include <cstring>
@@ -12,12 +12,16 @@
#include "stopwatch.hpp"
#include "ugv_sdk/details/agilex_msg_parser.h"
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot {
void TracerBase::Connect(std::string dev_name) {
AgilexBase::Connect(dev_name, std::bind(&TracerBase::ParseCANFrame, this,
std::placeholders::_1));
AgilexBase::ConnectPort(dev_name, std::bind(&TracerBase::ParseCANFrame, this,
std::placeholders::_1));
}
void TracerBase::Connect(std::string uart_name, uint32_t baudrate) {
// TODO
}
void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) {

160
src/stopwatch.hpp
View File

@@ -1,160 +0,0 @@
/*
* stopwatch.hpp
*
* Created on: Jul 12, 2020 12:07
* Description:
*
* Source:
* [1] https://github.com/sailormoon/stopwatch
* [2] https://github.com/rxdu/stopwatch
*
* Copyright (c) 2019 sailormoon <http://unlicense.org>
* Copyright (c) 2020 Ruixiang Du (rdu)
*
* License: <http://unlicense.org>
*/
#ifndef STOPWATCH_HPP
#define STOPWATCH_HPP
#include <array>
#include <chrono>
#include <cstdint>
#include <algorithm>
#include <thread>
namespace westonrobot {
// only supported on x86 processors
#if (defined __x86_64__) || (defined __i386)
// An implementation of the 'TrivialClock' concept using the rdtscp instruction.
struct rdtscp_clock {
using rep = std::uint64_t;
using period = std::ratio<1>;
using duration = std::chrono::duration<rep, period>;
using time_point = std::chrono::time_point<rdtscp_clock, duration>;
static time_point now() noexcept {
std::uint32_t hi, lo;
__asm__ __volatile__("rdtscp" : "=d"(hi), "=a"(lo));
return time_point(duration((static_cast<std::uint64_t>(hi) << 32) | lo));
}
};
// A timer using the specified clock.
template <class Clock = std::chrono::system_clock>
struct timer {
using time_point = typename Clock::time_point;
using duration = typename Clock::duration;
timer(const duration duration) : expiry(Clock::now() + duration) {}
timer(const time_point expiry) : expiry(expiry) {}
bool done() const { return done(Clock::now()); }
bool done(const time_point now) const { return now >= expiry; }
duration remaining() const { return remaining(Clock::now()); };
duration remaining(const time_point now) const { return expiry - now; }
const time_point expiry;
};
template <class Clock = std::chrono::system_clock>
constexpr timer<Clock> make_timer(const typename Clock::duration duration) {
return timer<Clock>(duration);
}
// Times how long it takes a function to execute using the specified clock.
template <class Clock = rdtscp_clock, class Func>
typename Clock::duration time_func(Func &&function) {
const auto start = Clock::now();
function();
return Clock::now() - start;
}
// Samples the given function N times using the specified clock.
template <std::size_t N, class Clock = rdtscp_clock, class Func>
std::array<typename Clock::duration, N> sample(Func &&function) {
std::array<typename Clock::duration, N> samples;
for (std::size_t i = 0u; i < N; ++i) {
samples[i] = time_func<Clock>(function);
}
std::sort(samples.begin(), samples.end());
return samples;
}
#endif /* __x86_64__ or __i386 */
struct StopWatch {
using Clock = std::chrono::high_resolution_clock;
using time_point = typename Clock::time_point;
using duration = typename Clock::duration;
StopWatch() { tic_point = Clock::now(); };
time_point tic_point;
void tic() { tic_point = Clock::now(); };
double toc() {
return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() -
tic_point)
.count() /
1000000.0;
};
// toc() in different units
double stoc() {
return std::chrono::duration_cast<std::chrono::seconds>(Clock::now() -
tic_point)
.count();
};
double mtoc() {
return std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() -
tic_point)
.count();
};
double utoc() {
return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() -
tic_point)
.count();
};
double ntoc() {
return std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() -
tic_point)
.count();
};
};
struct Timer {
using Clock = std::chrono::high_resolution_clock;
using time_point = typename Clock::time_point;
using duration = typename Clock::duration;
Timer() { tic_point = Clock::now(); };
time_point tic_point;
void reset() { tic_point = Clock::now(); };
// you have to call reset() before calling sleep functions
void sleep_until_ms(int64_t period_ms) {
int64_t duration =
period_ms - std::chrono::duration_cast<std::chrono::milliseconds>(
Clock::now() - tic_point)
.count();
if (duration > 0)
std::this_thread::sleep_for(std::chrono::milliseconds(duration));
};
void sleep_until_us(int64_t period_us) {
int64_t duration =
period_us - std::chrono::duration_cast<std::chrono::microseconds>(
Clock::now() - tic_point)
.count();
if (duration > 0)
std::this_thread::sleep_for(std::chrono::microseconds(duration));
};
};
} // namespace westonrobot
#endif // STOPWATCH_HPP