DifferentialCPG::DifferentialCPG( const ::gazebo::physics::ModelPtr &_model, const sdf::ElementPtr _settings, const std::vector< revolve::gazebo::MotorPtr > &_motors, const std::vector< revolve::gazebo::SensorPtr > &_sensors) : nextState_(nullptr) , input_(new double[_sensors.size()]) , output_(new double[_motors.size()]) { // Create transport node this->node_.reset(new gz::transport::Node()); this->node_->Init(); auto name = _model->GetName(); // Listen to network modification requests // alterSub_ = node_->Subscribe( // "~/" + name + "/modify_diff_cpg", &DifferentialCPG::Modify, // this); if (not _settings->HasElement("rv:brain")) { std::cerr << "No robot brain detected, this is probably an error." << std::endl; throw std::runtime_error("DifferentialCPG brain did not receive settings"); } std::cout << _settings->GetDescription() << std::endl; auto motor = _settings->HasElement("rv:motor") ? _settings->GetElement("rv:motor") : sdf::ElementPtr(); while(motor) { if (not motor->HasAttribute("x") or not motor->HasAttribute("y")) { std::cerr << "Missing required motor attributes (x- and/or y- coordinate)" << std::endl; throw std::runtime_error("Robot brain error"); } auto motorId = motor->GetAttribute("part_id")->GetAsString(); auto coordX = std::atoi(motor->GetAttribute("x")->GetAsString().c_str()); auto coordY = std::atoi(motor->GetAttribute("y")->GetAsString().c_str()); this->positions_[motorId] = {coordX, coordY}; this->neurons_[{coordX, coordY, 1}] = {1.f, 0.f, 0.f}; this->neurons_[{coordX, coordY, -1}] = {1.f, 0.f, 0.f}; // TODO: Add check for duplicate coordinates motor = motor->GetNextElement("rv:motor"); } // Add connections between neighbouring neurons for (const auto &position : this->positions_) { auto name = position.first; int x, y; std::tie(x, y) = position.second; if (this->connections_.count({x, y, 1, x, y, -1})) { continue; } if (this->connections_.count({x, y, -1, x, y, 1})) { continue; } this->connections_[{x, y, 1, x, y, -1}] = 1.f; this->connections_[{x, y, -1, x, y, 1}] = 1.f; for (const auto &neighbour : this->positions_) { int nearX, nearY; std::tie(nearX, nearY) = neighbour.second; if ((x+1) == nearX or (y+1) == nearY or (x-1) == nearX or (y-1) == nearY) { this->connections_[{x, y, 1, nearX, nearY, 1}] = 1.f; this->connections_[{nearX, nearY, 1, x, y, 1}] = 1.f; } } } // Initialise array of neuron states for Update() method this->nextState_ = new double[this->neurons_.size()]; }