99 lines
4.0 KiB
C++
99 lines
4.0 KiB
C++
#ifndef CMVR_ES_MOTOR_MANAGER_H
|
|
#define CMVR_ES_MOTOR_MANAGER_H
|
|
|
|
#include <cstdint>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <string>
|
|
#include <unordered_set>
|
|
#include <unordered_map>
|
|
#include <vector>
|
|
|
|
#include "devices/abstract_device.h"
|
|
#include "cmvr/config/motor_config/motor_config.pb.h"
|
|
#include "motor/abstract_motor.h"
|
|
|
|
namespace cmvr::msgs {
|
|
|
|
class RobotDetail;
|
|
|
|
} // namespace cmvr::msgs
|
|
|
|
namespace cmvr::simulate {
|
|
|
|
class MujocoWorld;
|
|
|
|
} // namespace cmvr::simulate
|
|
|
|
namespace cmvr::device {
|
|
|
|
class AbstractMotorBusRuntime;
|
|
|
|
class MotorManager final : public AbstractDevice,
|
|
public std::enable_shared_from_this<MotorManager> {
|
|
public:
|
|
MotorManager(std::string id, const config::MotorConfig& cfg);
|
|
~MotorManager() override;
|
|
|
|
DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; }
|
|
std::string typeName() const override { return "MotorManager"; }
|
|
|
|
bool init() override;
|
|
bool stop() override;
|
|
|
|
bool addMotor(const std::shared_ptr<AbstractMotor>& motor);
|
|
std::shared_ptr<AbstractMotor> getMotor(std::uint8_t node_id) const;
|
|
std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const;
|
|
const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() const;
|
|
|
|
static std::shared_ptr<MotorManager> managerFor(const std::string& id);
|
|
static std::shared_ptr<simulate::MujocoWorld> mujocoWorldFor(const std::string& id);
|
|
static void setActiveJoints(const std::string& motor_manager_id,
|
|
std::unordered_map<std::string, std::unordered_set<std::string>> group_joints);
|
|
static void clearActiveJoints();
|
|
|
|
private:
|
|
using ActiveJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
|
|
|
|
bool selectActiveMotors_(const std::string& group_name,
|
|
const google::protobuf::RepeatedPtrField<config::MotorConfigItem>& source,
|
|
std::vector<config::MotorConfigItem>& selected) const;
|
|
bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
|
|
std::vector<config::MotorConfigItem>& selected) const;
|
|
std::shared_ptr<AbstractMotorBusRuntime> createBusRuntime_(
|
|
const config::MotorGroupConfig& group_cfg) const;
|
|
std::vector<std::shared_ptr<AbstractMotor>> createMotors_(
|
|
const config::MotorGroupConfig& group_cfg,
|
|
const std::vector<config::MotorConfigItem>& motor_cfgs,
|
|
const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
|
|
std::vector<std::shared_ptr<AbstractMotor>> createCanMotors_(
|
|
const config::MotorGroupConfig& group_cfg,
|
|
const std::vector<config::MotorConfigItem>& motor_cfgs,
|
|
const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
|
|
std::vector<std::shared_ptr<AbstractMotor>> createMujocoMotors_(
|
|
const config::MotorGroupConfig& group_cfg,
|
|
const std::vector<config::MotorConfigItem>& motor_cfgs,
|
|
const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
|
|
std::vector<std::shared_ptr<AbstractMotor>> createEthercatMotors_(
|
|
const config::MotorGroupConfig& group_cfg,
|
|
const std::vector<config::MotorConfigItem>& motor_cfgs,
|
|
const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
|
|
|
|
private:
|
|
config::MotorConfig cfg_;
|
|
std::vector<std::shared_ptr<AbstractMotorBusRuntime>> bus_runtimes_;
|
|
mutable std::mutex motors_mutex_;
|
|
std::unordered_map<std::uint8_t, std::shared_ptr<AbstractMotor>> motors_by_id_;
|
|
std::unordered_map<std::string, std::shared_ptr<AbstractMotor>> motors_by_joint_;
|
|
bool initialized_{false};
|
|
|
|
static std::mutex registry_mutex_;
|
|
static std::unordered_map<std::string, std::weak_ptr<MotorManager>> managers_;
|
|
static std::unordered_map<std::string, std::weak_ptr<simulate::MujocoWorld>> mujoco_world_registry_;
|
|
static std::unordered_map<std::string, ActiveJointSelection> active_joints_;
|
|
};
|
|
|
|
} // namespace cmvr::device
|
|
|
|
#endif // CMVR_ES_MOTOR_MANAGER_H
|