68 lines
2.2 KiB
C++
68 lines
2.2 KiB
C++
//
|
|
// Created by linbo on 2025/10/22.
|
|
//
|
|
|
|
#include "hardware/can/can_group.h"
|
|
#include <glog/logging.h>
|
|
using namespace std;
|
|
using namespace cmvr::hardware;
|
|
|
|
CanGroup::CanGroup(const XmlNode &cfg):cfg_(cfg)
|
|
{
|
|
try {
|
|
id_ = cfg.getAttrString("id");
|
|
channelId_ = cfg.getAttrDefault("channelId",0);
|
|
enable_ = cfg.getAttrDefault("enable", false);
|
|
|
|
}
|
|
catch (const exception &e) {
|
|
LOG(FATAL) << "[CanGroup] (CanGroup): CanGroup initialization failed: " << e.what();
|
|
throw std::runtime_error("[CanGroup] (CanGroup): CanGroup create failed" + string(e.what()));
|
|
}
|
|
}
|
|
|
|
|
|
void CanGroup::init()
|
|
{
|
|
can_client_ = std::make_shared<device::SocketCanClientRaw>(cfg_);
|
|
can_sender_ = std::make_shared<device::CanSender<msgs::RobotDetail> >();
|
|
can_receiver_ = std::make_shared<device::CanReceiver<msgs::RobotDetail> >();
|
|
message_manager_ = std::make_shared<device::MessageManager<msgs::RobotDetail> >();
|
|
|
|
//初始化protocol协议实例
|
|
can_client_->init();
|
|
auto ret = can_sender_->Init(can_client_.get(), false);
|
|
if (ret != device::ErrorCode::OK)
|
|
LOG(ERROR) << "Failed to init " << id_ << " CAN sender.";
|
|
ret = can_receiver_->Init(can_client_.get(), message_manager_.get(), false);
|
|
if (ret != device::ErrorCode::OK)
|
|
LOG(ERROR) << "Failed to init " << id_ << " CAN receiver.";
|
|
can_client_->start();
|
|
ret = can_sender_->Start();
|
|
if (ret != device::ErrorCode::OK)
|
|
LOG(ERROR) << "Failed to start " << id_ << " CAN sender.";
|
|
|
|
ret = can_receiver_->Start();
|
|
if (ret != device::ErrorCode::OK)
|
|
LOG(ERROR) << "Failed to start " << id_ << " CAN receiver.";
|
|
|
|
|
|
motor_protocol_manager = std::make_shared<MotorProtocolManager>(cfg_,can_sender_,message_manager_);
|
|
|
|
}
|
|
|
|
std::shared_ptr<AbstractMotorProtocol> CanGroup::getMotorProtocol(const std::string &protocolType)
|
|
{
|
|
return motor_protocol_manager->getMotorProtocol(protocolType);
|
|
}
|
|
|
|
uint8_t CanGroup::getNodeId(const std::string& joint_name)
|
|
{
|
|
return motor_protocol_manager->getNodeId(joint_name);
|
|
}
|
|
|
|
const MotorProtocolManager::MotorInfo& CanGroup::getMotorInfo(const std::string& joint_name)
|
|
{
|
|
return motor_protocol_manager->getMotorInfo(joint_name);
|
|
}
|