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 18:20:34 +08:00
parent d2f210e6c1
commit dd810eccba
13 changed files with 148 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -77,7 +77,7 @@ add_library(${PROJECT_NAME}
src/async_port/async_can.cpp
########################
## public interface to access robot
# src/mobile_base/scout_robot.cpp
src/mobile_base/scout_robot.cpp
########################
## protocol v2 support
# parser

4
demo/scout_demo/CMakeLists.txt
View File

@@ -2,5 +2,5 @@
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)
add_executable(demo_scout_robot scout_robot_demo.cpp)
target_link_libraries(demo_scout_robot ugv_sdk)

86
demo/scout_demo/scout_robot_demo.cpp
View File

@@ -25,54 +25,54 @@ int main(int argc, char **argv) {
return -1;
}
ScoutRobot scout(AgilexProtocol::V1);
// scout.Connect(device_name);
ScoutRobot scout(ProtocolType::AGX_V2);
// scout.Connect(device_name);
// scout.EnableCommandedMode();
// 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();
// // // 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 < 100) {
// std::cout << "Motor: 0.2, 0" << std::endl;
// scout.SetMotionCommand(0.2, 0.0);
// }
// int count = 0;
// while (true) {
// // motion control
// if (count < 100) {
// std::cout << "Motor: 0.2, 0" << std::endl;
// scout.SetMotionCommand(0.2, 0.0);
// }
// // get robot state
// auto state = scout.GetScoutState();
// 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;
// // get robot state
// auto state = scout.GetScoutState();
// 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;
// }
// usleep(20000);
// ++count;
// }
return 0;
}

2
demo/scout_demo/scout_v2_demo.cpp
View File

@@ -23,7 +23,7 @@ int main(int argc, char **argv) {
return -1;
}
ScoutBase scout;
ScoutBaseV2 scout;
scout.Connect(device_name);
scout.EnableCommandedMode();

0
include/ugv_sdk/mobile_base/ranger_interface.hpp → include/ugv_sdk/interface/ranger_interface.hpp
View File

40
include/ugv_sdk/interface/robot_interface.hpp
View File

@@ -15,25 +15,41 @@
#include "ugv_sdk/interface/agilex_message.h"
namespace westonrobot {
struct RobotInterface {
enum class ProtocolType { AGX_V1, AGX_V2 };
class RobotInterface {
public:
~RobotInterface() = default;
// functions to be implemented by class AgilexBase
virtual void EnableCommandedMode() = 0;
virtual void SetMotionMode(uint8_t mode){};
virtual void SendMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity,
double steering_angle) = 0;
virtual void SendLightCommand(LightMode front_mode,
uint8_t front_custom_value, LightMode rear_mode,
uint8_t rear_custom_value) = 0;
virtual void DisableLightControl() = 0;
// functions to be implemented by each robot class
virtual void Connect(std::string can_name) = 0;
virtual void Connect(std::string uart_name, uint32_t baudrate) = 0;
// functions to be implemented by each robot class
virtual void Connect(std::string uart_name, uint32_t baudrate){
// use derived version
};
virtual void ResetRobotState() = 0;
protected:
/****** functions not available/valid to all robots ******/
// functions to be implemented by class AgilexBase
virtual void SetMotionMode(uint8_t mode){};
// any specific robot will use a specialized version of the two functions
virtual void SendMotionCommand(double linear_vel, double angular_vel,
double lateral_velocity,
double steering_angle){
// use derived version
};
virtual void SendLightCommand(LightMode front_mode,
uint8_t front_custom_value, LightMode rear_mode,
uint8_t rear_custom_value){
// use derived version
};
};
} // namespace westonrobot

2
include/ugv_sdk/interface/scout_interface.hpp
View File

@@ -31,6 +31,8 @@ struct ScoutState {
};
struct ScoutInterface {
~ScoutInterface() = default;
virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
virtual void SetLightCommand(LightMode f_mode, uint8_t f_value,
LightMode r_mode, uint8_t r_value) = 0;

0
include/ugv_sdk/mobile_base/tracer_interface.hpp → include/ugv_sdk/interface/tracer_interface.hpp
View File

2
include/ugv_sdk/mobile_base/agilex_base.hpp
View File

@@ -29,8 +29,6 @@
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot {
enum class AgilexProtocol { V1, V2 };
template <typename ParserType>
class AgilexBase : public RobotInterface {
public:

25
include/ugv_sdk/mobile_base/scout_robot.hpp
View File

@@ -12,15 +12,32 @@
#include <memory>
#include "ugv_sdk/mobile_base/common.hpp"
#include "ugv_sdk/interface/robot_interface.hpp"
#include "ugv_sdk/interface/scout_interface.hpp"
namespace westonrobot {
class ScoutRobot {
class ScoutRobot : public RobotInterface, public ScoutInterface {
public:
ScoutRobot(AgilexProtocol protocol);
ScoutRobot(ProtocolType protocol = ProtocolType::AGX_V2);
~ScoutRobot();
void Connect(std::string can_name) override;
void Connect(std::string uart_name, uint32_t baudrate) override;
void EnableCommandedMode();
void SetMotionCommand(double linear_vel, double angular_vel) override;
void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value) override;
void DisableLightControl() override;
void ResetRobotState() override;
// get robot state
ScoutState GetRobotState() override;
private:
std::unique_ptr<ScoutInterface> robot_;
RobotInterface* robot_;
};
} // namespace westonrobot

6
include/ugv_sdk/protocol_v2/scout_base_v2.hpp
View File

@@ -21,10 +21,10 @@
#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp"
namespace westonrobot {
class ScoutBase : public AgilexBase<ProtocolV2Parser>, public ScoutInterface {
class ScoutBaseV2 : public AgilexBase<ProtocolV2Parser>, public ScoutInterface {
public:
ScoutBase() : AgilexBase(){};
~ScoutBase() = default;
ScoutBaseV2() : AgilexBase(){};
~ScoutBaseV2() = default;
// set up connection
void Connect(std::string can_name) override;

40
src/mobile_base/scout_robot.cpp
View File

@@ -8,7 +8,45 @@
*/
#include "ugv_sdk/mobile_base/scout_robot.hpp"
#include "ugv_sdk/protocol_v2/scout_base_v2.hpp"
namespace westonrobot {
ScoutRobot::ScoutRobot(AgilexProtocol protocol) {}
ScoutRobot::ScoutRobot(ProtocolType protocol) {
if (protocol == ProtocolType::AGX_V1) {
} else if (protocol == ProtocolType::AGX_V2) {
robot_ = new ScoutBaseV2();
}
}
ScoutRobot::~ScoutRobot() {
if (robot_) delete robot_;
}
void ScoutRobot::EnableCommandedMode() { robot_->EnableCommandedMode(); }
void ScoutRobot::Connect(std::string can_name) { robot_->Connect(can_name); }
void ScoutRobot::Connect(std::string uart_name, uint32_t baudrate) {
robot_->Connect(uart_name, baudrate);
}
void ScoutRobot::ResetRobotState() { robot_->ResetRobotState(); }
void ScoutRobot::SetMotionCommand(double linear_vel, double angular_vel) {
auto scout = dynamic_cast<ScoutInterface*>(robot_);
scout->SetMotionCommand(linear_vel, angular_vel);
}
void ScoutRobot::DisableLightControl() { robot_->DisableLightControl(); }
void ScoutRobot::SetLightCommand(LightMode f_mode, uint8_t f_value,
LightMode r_mode, uint8_t r_value) {
auto scout = dynamic_cast<ScoutInterface*>(robot_);
scout->SetLightCommand(f_mode, f_value, r_mode, r_value);
}
ScoutState ScoutRobot::GetRobotState() {
auto scout = dynamic_cast<ScoutInterface*>(robot_);
return scout->GetRobotState();
}
} // namespace westonrobot

16
src/protocol_v2/scout_base.cpp
View File

@@ -22,37 +22,37 @@
#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
namespace westonrobot {
void ScoutBase::Connect(std::string dev_name) {
AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBase::ParseCANFrame, this,
void ScoutBaseV2::Connect(std::string dev_name) {
AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBaseV2::ParseCANFrame, this,
std::placeholders::_1));
}
void ScoutBase::Connect(std::string uart_name, uint32_t baudrate) {
void ScoutBaseV2::Connect(std::string uart_name, uint32_t baudrate) {
// TODO
}
void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel) {
void ScoutBaseV2::SetMotionCommand(double linear_vel, double angular_vel) {
AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
}
void ScoutBase::SetLightCommand(LightMode f_mode, uint8_t f_value,
void ScoutBaseV2::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::GetRobotState() {
ScoutState ScoutBaseV2::GetRobotState() {
std::lock_guard<std::mutex> guard(state_mutex_);
return scout_state_;
}
void ScoutBase::ParseCANFrame(can_frame *rx_frame) {
void ScoutBaseV2::ParseCANFrame(can_frame *rx_frame) {
AgxMessage status_msg;
DecodeCanFrame(rx_frame, &status_msg);
std::lock_guard<std::mutex> guard(state_mutex_);
UpdateScoutState(status_msg, scout_state_);
}
void ScoutBase::UpdateScoutState(const AgxMessage &status_msg,
void ScoutBaseV2::UpdateScoutState(const AgxMessage &status_msg,
ScoutState &state) {
switch (status_msg.type) {
case AgxMsgSystemState: {