From dee7e6bc3b5a51d0652f05877fd2697c1d6b0ada Mon Sep 17 00:00:00 2001 From: Milan Jelisavcic Date: Sun, 6 Aug 2017 01:27:56 +0200 Subject: [PATCH] Change codestyle in order to make the world a better place --- cpp/revolve/gazebo/brain/NeuralNetwork.cpp | 1262 +++++++++-------- cpp/revolve/gazebo/brain/NeuralNetwork.h | 292 ++-- cpp/revolve/gazebo/motors/JointMotor.cpp | 62 +- cpp/revolve/gazebo/motors/Motor.cpp | 99 +- cpp/revolve/gazebo/motors/MotorFactory.cpp | 94 +- cpp/revolve/gazebo/motors/PositionMotor.cpp | 196 +-- cpp/revolve/gazebo/motors/VelocityMotor.cpp | 104 +- cpp/revolve/gazebo/plugin/BodyAnalyzer.cpp | 512 ++++--- cpp/revolve/gazebo/plugin/RobotController.cpp | 375 ++--- cpp/revolve/gazebo/plugin/WorldController.cpp | 385 ++--- cpp/revolve/gazebo/sensors/BatterySensor.cpp | 62 +- cpp/revolve/gazebo/sensors/ImuSensor.cpp | 87 +- cpp/revolve/gazebo/sensors/LightSensor.cpp | 95 +- .../gazebo/sensors/PointIntensitySensor.cpp | 90 +- cpp/revolve/gazebo/sensors/Sensor.cpp | 83 +- cpp/revolve/gazebo/sensors/SensorFactory.cpp | 122 +- cpp/revolve/gazebo/sensors/TouchSensor.cpp | 97 +- cpp/revolve/gazebo/sensors/VirtualSensor.cpp | 61 +- .../standalone/body_analyzer_server.cpp | 17 +- 19 files changed, 2286 insertions(+), 1809 deletions(-) diff --git a/cpp/revolve/gazebo/brain/NeuralNetwork.cpp b/cpp/revolve/gazebo/brain/NeuralNetwork.cpp index 8a17908817..e9b96dc846 100644 --- a/cpp/revolve/gazebo/brain/NeuralNetwork.cpp +++ b/cpp/revolve/gazebo/brain/NeuralNetwork.cpp @@ -21,616 +21,768 @@ #include #include #include +#include +#include #include -#include +#include +#include #include "../motors/Motor.h" -#include "../sensors/Sensor.h" #include "NeuralNetwork.h" namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -// Internal helper function to build neuron params -void neuronHelper(double* params, unsigned int* types, sdf::ElementPtr neuron); -void neuronHelper(double* params, unsigned int* types, const revolve::msgs::Neuron & neuron); - -NeuralNetwork::NeuralNetwork(std::string modelName, - sdf::ElementPtr node, - std::vector< MotorPtr > & motors, - std::vector< SensorPtr > & sensors) - : flipState_(false) - , nInputs_(0) - , nOutputs_(0) - , nHidden_(0) - , nNonInputs_(0) +namespace revolve { - // Create transport node - node_.reset(new gz::transport::Node()); - node_->Init(); - - // Listen to network modification requests - alterSub_ = node_->Subscribe("~/"+modelName+"/modify_neural_network", - &NeuralNetwork::modify, this); - - // Initialize weights, input and states to zero by default - memset(inputWeights_, 0, sizeof(inputWeights_)); - memset(outputWeights_, 0, sizeof(outputWeights_)); - memset(hiddenWeights_, 0, sizeof(hiddenWeights_)); - memset(state1_, 0, sizeof(state1_)); - memset(state2_, 0, sizeof(state2_)); - memset(input_, 0, sizeof(input_)); - - // We now setup the neural network and its parameters. The end result - // of this operation should be that we can iterate/update all sensors in - // a straightforward manner, likewise for the motors. We therefore first - // create a map of all neurons, telling us how many there are for each - // type, and what their properties are. We then iterate all sensors and - // motors, creating the adequate neurons in place as we do so. - - // Map of ID to neuron element - std::map neuronMap; - - // List of all hidden neurons for convenience - std::vector hiddenNeurons; - - // Set for tracking all collected input/output neurons - std::set toProcess; - - // Fetch the first neuron; note the HasElement call is necessary to prevent SDF from complaining - // if no neurons are present. - auto neuron = node->HasElement("rv:neuron") ? node->GetElement("rv:neuron") : sdf::ElementPtr(); - while (neuron) { - if (not neuron->HasAttribute("layer") || not neuron->HasAttribute("id")) { - std::cerr << "Missing required neuron attributes (id or layer). '" << std::endl; - throw std::runtime_error("Robot brain error"); - } - auto layer = neuron->GetAttribute("layer")->GetAsString(); - auto neuronId = neuron->GetAttribute("id")->GetAsString(); - - if (layerMap_.count(neuronId)) { - std::cerr << "Duplicate neuron ID '" - << neuronId << "'" << std::endl; - throw std::runtime_error("Robot brain error"); - } - - layerMap_[neuronId] = layer; - neuronMap[neuronId] = neuron; + namespace gazebo + { + // Internal helper function to build neuron params + void neuronHelper(double *params, + unsigned int *types, + sdf::ElementPtr neuron); + + void neuronHelper(double *params, + unsigned int *types, + const revolve::msgs::Neuron &neuron); + + NeuralNetwork::NeuralNetwork(std::string modelName, + sdf::ElementPtr node, + std::vector< MotorPtr > &motors, + std::vector< SensorPtr > &sensors) + : flipState_(false) + , nInputs_(0) + , nOutputs_(0) + , nHidden_(0) + , nNonInputs_(0) + { + // Create transport node + node_.reset(new gz::transport::Node()); + node_->Init(); + + // Listen to network modification requests + alterSub_ = node_->Subscribe("~/" + modelName + "/modify_neural_network", + &NeuralNetwork::modify, this); + + // Initialize weights, input and states to zero by default + memset(inputWeights_, 0, sizeof(inputWeights_)); + memset(outputWeights_, 0, sizeof(outputWeights_)); + memset(hiddenWeights_, 0, sizeof(hiddenWeights_)); + memset(state1_, 0, sizeof(state1_)); + memset(state2_, 0, sizeof(state2_)); + memset(input_, 0, sizeof(input_)); + + // We now setup the neural network and its parameters. The end result + // of this operation should be that we can iterate/update all sensors in + // a straightforward manner, likewise for the motors. We therefore first + // create a map of all neurons, telling us how many there are for each + // type, and what their properties are. We then iterate all sensors and + // motors, creating the adequate neurons in place as we do so. + + // Map of ID to neuron element + std::map< std::string, sdf::ElementPtr > neuronMap; + + // List of all hidden neurons for convenience + std::vector< std::string > hiddenNeurons; + + // Set for tracking all collected input/output neurons + std::set< std::string > toProcess; + + // Fetch the first neuron; note the HasElement call is necessary to + // prevent SDF from complaining + // if no neurons are present. + auto neuron = node->HasElement("rv:neuron") ? + node->GetElement("rv:neuron") : + sdf::ElementPtr(); + while (neuron) + { + if (not neuron->HasAttribute("layer") || not neuron->HasAttribute("id")) + { + std::cerr + << "Missing required neuron attributes (id or layer). '" + << std::endl; + throw std::runtime_error("Robot brain error"); + } + auto layer = neuron->GetAttribute("layer")->GetAsString(); + auto neuronId = neuron->GetAttribute("id")->GetAsString(); + + if (layerMap_.count(neuronId)) + { + std::cerr << "Duplicate neuron ID '" + << neuronId << "'" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + layerMap_[neuronId] = layer; + neuronMap[neuronId] = neuron; + + if ("input" == layer) + { + if (nInputs_ >= MAX_INPUT_NEURONS) + { + std::cerr << "The number of input neurons(" << (nInputs_ + 1) + << ") is greater than the maximum allowed one (" + << MAX_INPUT_NEURONS << ")" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + toProcess.insert(neuronId); + nInputs_++; + } + else if ("output" == layer) + { + if (nOutputs_ >= MAX_OUTPUT_NEURONS) + { + std::cerr << "The number of output neurons(" << (nOutputs_ + 1) + << ") is greater than the maximum allowed one (" + << MAX_OUTPUT_NEURONS << ")" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + toProcess.insert(neuronId); + nOutputs_++; + } + else if ("hidden" == layer) + { + if (hiddenNeurons.size() >= MAX_HIDDEN_NEURONS) + { + std::cerr + << "The number of hidden neurons(" + << (hiddenNeurons.size() + 1) + << ") is greater than the maximum allowed one (" + << MAX_HIDDEN_NEURONS + << ")" + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + hiddenNeurons.push_back(neuronId); + nHidden_++; + } + else + { + std::cerr << "Unknown neuron layer '" << layer << "'." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + neuron = neuron->GetNextElement("rv:neuron"); + } - if ("input" == layer) { - if (nInputs_ >= MAX_INPUT_NEURONS) { - std::cerr << "The number of input neurons(" << (nInputs_ + 1) - << ") is greater than the maximum allowed one (" - << MAX_INPUT_NEURONS << ")" << std::endl; - throw std::runtime_error("Robot brain error"); + // Create motor output neurons at the correct position + // We iterate a part's motors and just assign every + // neuron we find in order. + std::map< std::string, unsigned int > outputCountMap; + unsigned int outPos = 0; + for (auto it = motors.begin(); it != motors.end(); ++it) + { + auto motor = *it; + auto partId = motor->partId(); + if (not outputCountMap.count(partId)) + { + outputCountMap[partId] = 0; + } + + for (unsigned int i = 0, l = motor->outputs(); i < l; ++i) + { + std::stringstream neuronId; + neuronId << partId << "-out-" << outputCountMap[partId]; + outputCountMap[partId]++; + + auto details = neuronMap.find(neuronId.str()); + if (details == neuronMap.end()) + { + std::cerr << "Required output neuron " << neuronId.str() << + " for motor could not be located" + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + neuronHelper(¶ms_[outPos * MAX_NEURON_PARAMS], + &types_[outPos], + details->second); + positionMap_[neuronId.str()] = outPos; + toProcess.erase(neuronId.str()); + outPos++; + } } - toProcess.insert(neuronId); - nInputs_++; - } else if ("output" == layer) { - if (nOutputs_ >= MAX_OUTPUT_NEURONS) { - std::cerr << "The number of output neurons(" << (nOutputs_ + 1) - << ") is greater than the maximum allowed one (" - << MAX_OUTPUT_NEURONS << ")" << std::endl; - throw std::runtime_error("Robot brain error"); + // Create sensor input neurons + std::map< std::string, unsigned int > inputCountMap; + unsigned int inPos = 0; + for (auto it = sensors.begin(); it != sensors.end(); ++it) + { + auto sensor = *it; + auto partId = sensor->partId(); + + if (not inputCountMap.count(partId)) + { + inputCountMap[partId] = 0; + } + + for (unsigned int i = 0, l = sensor->inputs(); i < l; ++i) + { + std::stringstream neuronId; + neuronId << partId << "-in-" << inputCountMap[partId]; + inputCountMap[partId]++; + + auto details = neuronMap.find(neuronId.str()); + if (details == neuronMap.end()) + { + std::cerr << "Required input neuron " << neuronId.str() << + " for sensor could not be located" + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + // Input neurons can currently not have a type, so + // there is no need to process it. + positionMap_[neuronId.str()] = inPos; + toProcess.erase(neuronId.str()); + inPos++; + } } - toProcess.insert(neuronId); - nOutputs_++; - } else if ("hidden" == layer) { - if (hiddenNeurons.size() >= MAX_HIDDEN_NEURONS) { - std::cerr << "The number of hidden neurons(" << (hiddenNeurons.size() + 1) - << ") is greater than the maximum allowed one (" - << MAX_HIDDEN_NEURONS << ")" << std::endl; + // Check if there are any input / output neurons which have not + // yet been processed. This is an error - every input / output + // neuron should be connected to at least a virtual motor / sensor. + if (toProcess.size()) + { + std::cerr + << "The following input / output neurons were" + " defined, but not attached to any sensor / motor:" + << std::endl; + + for (auto it = toProcess.begin(); it != toProcess.end(); ++it) + { + std::cerr << (*it) << std::endl; + } + + std::cerr << "Create virtual sensor and motors for input / output" + " neurons that you would like to control manually."; throw std::runtime_error("Robot brain error"); } - hiddenNeurons.push_back(neuronId); - nHidden_++; - } else { - std::cerr << "Unknown neuron layer '" << layer << "'." << std::endl; - throw std::runtime_error("Robot brain error"); - } - - neuron = neuron->GetNextElement("rv:neuron"); - } - - // Create motor output neurons at the correct position - // We iterate a part's motors and just assign every - // neuron we find in order. - std::map outputCountMap; - unsigned int outPos = 0; - for (auto it = motors.begin(); it != motors.end(); ++it) { - auto motor = *it; - auto partId = motor->partId(); - if (not outputCountMap.count(partId)) { - outputCountMap[partId] = 0; - } - - for (unsigned int i = 0, l = motor->outputs(); i < l; ++i) { - std::stringstream neuronId; - neuronId << partId << "-out-" << outputCountMap[partId]; - outputCountMap[partId]++; - - auto details = neuronMap.find(neuronId.str()); - if (details == neuronMap.end()) { - std::cerr << "Required output neuron " << neuronId.str() << - " for motor could not be located" - << std::endl; - throw std::runtime_error("Robot brain error"); + // Add hidden neurons + outPos = 0; + for (auto it = hiddenNeurons.begin(); it != hiddenNeurons.end(); ++it) + { + auto neuronId = *it; + + // Position relative to hidden neurons + positionMap_[neuronId] = outPos; + + // Offset with output neurons within params / types + auto pos = nOutputs_ + outPos; + neuronHelper(¶ms_[pos * MAX_NEURON_PARAMS], + &types_[pos], + neuronMap[neuronId]); + outPos++; } - neuronHelper(¶ms_[outPos * MAX_NEURON_PARAMS], &types_[outPos], details->second); - positionMap_[neuronId.str()] = outPos; - toProcess.erase(neuronId.str()); - outPos++; + // Decode connections + nNonInputs_ = nOutputs_ + nHidden_; + auto connection = + node->HasElement("rv:neural_connection") ? node->GetElement( + "rv:neural_connection") : sdf::ElementPtr(); + while (connection) + { + if (not connection->HasAttribute("src") || not connection->HasAttribute( + "dst") + || not connection->HasAttribute("weight")) + { + std::cerr << "Missing required connection attributes " + "(`src`, `dst` or `weight`)." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto src = connection->GetAttribute("src")->GetAsString(); + auto dst = connection->GetAttribute("dst")->GetAsString(); + double weight; + connection->GetAttribute("weight")->Get(weight); + + // Use connection helper to set the weight + connectionHelper(src, dst, weight); + + // Load the next connection + connection = connection->GetNextElement("rv:neural_connection"); + } } - } - // Create sensor input neurons - std::map inputCountMap; - unsigned int inPos = 0; - for (auto it = sensors.begin(); it != sensors.end(); ++it) { - auto sensor = *it; - auto partId = sensor->partId(); - - if (not inputCountMap.count(partId)) { - inputCountMap[partId] = 0; - } + NeuralNetwork::~NeuralNetwork() + {} - for (unsigned int i = 0, l = sensor->inputs(); i < l; ++i) { - std::stringstream neuronId; - neuronId << partId << "-in-" << inputCountMap[partId]; - inputCountMap[partId]++; + void NeuralNetwork::step(double time) + { + unsigned int i = 0; + unsigned int j = 0; - auto details = neuronMap.find(neuronId.str()); - if (details == neuronMap.end()) { - std::cerr << "Required input neuron " << neuronId.str() << - " for sensor could not be located" - << std::endl; - throw std::runtime_error("Robot brain error"); + if (nOutputs_ == 0) + { + return; } - // Input neurons can currently not have a type, so - // there is no need to process it. - positionMap_[neuronId.str()] = inPos; - toProcess.erase(neuronId.str()); - inPos++; - } - } + double *curState, *nextState; + if (flipState_) + { + curState = state2_; + nextState = state1_; + } + else + { + curState = state1_; + nextState = state2_; + } - // Check if there are any input / output neurons which have not - // yet been processed. This is an error - every input / output - // neuron should be connected to at least a virtual motor / sensor. - if (toProcess.size()) { - std::cerr << "The following input / output neurons were" - " defined, but not attached to any sensor / motor:" << std::endl; - for (auto it = toProcess.begin(); it != toProcess.end(); ++it) { - std::cerr << (*it) << std::endl; - } + unsigned int maxNonInputs = MAX_HIDDEN_NEURONS + MAX_OUTPUT_NEURONS; + for (i = 0; i < nNonInputs_; ++i) + { + double curNeuronActivation = 0; + + // Add input neuron values + for (j = 0; j < nInputs_; ++j) + { + curNeuronActivation += inputWeights_[maxNonInputs * j + i] + * input_[j]; + } + + // Add output neuron values + for (j = 0; j < nOutputs_; ++j) + { + curNeuronActivation += + outputWeights_[maxNonInputs * j + i] * curState[j]; + } + + // Add hidden neuron values + for (j = 0; j < nHidden_; ++j) + { + curNeuronActivation += + hiddenWeights_[maxNonInputs * j + i] + * curState[nOutputs_ + j]; + } + + unsigned int base = MAX_NEURON_PARAMS * i; + switch (types_[i]) + { + case SIGMOID: + /* params are bias, gain */ + curNeuronActivation -= params_[base]; + nextState[i] = 1.0 + / (1.0 + exp(-params_[base + 1] * + curNeuronActivation)); + break; + case SIMPLE: + /* linear, params are bias, gain */ + curNeuronActivation -= params_[base]; + nextState[i] = params_[base + 1] * + curNeuronActivation; + break; + case OSCILLATOR: + { // Use the block to prevent "crosses initialization" error + /* params are period, phase offset, gain (amplitude) */ + double period = params_[base]; + double phaseOffset = params_[base + 1]; + double gain = params_[base + 2]; + + /* Value in [0, 1] */ + nextState[i] = ((sin((2.0 * M_PI / period) * + (time - period * phaseOffset))) + 1.0) / 2.0; + + /* set output to be in [0.5 - gain/2, 0.5 + gain/2] */ + nextState[i] = (0.5 - (gain / 2.0) + + nextState[i] * gain); + } + break; + default: + // Unsupported type should never happen + std::cerr + << "Invalid neuron type during processing, must be a bug." + << std::endl; + throw std::runtime_error("Robot brain error"); + } + } - std::cerr << "Create virtual sensor and motors for input / output" - " neurons that you would like to control manually."; - throw std::runtime_error("Robot brain error"); - } - - // Add hidden neurons - outPos = 0; - for (auto it = hiddenNeurons.begin(); it != hiddenNeurons.end(); ++it) { - auto neuronId = *it; - - // Position relative to hidden neurons - positionMap_[neuronId] = outPos; - - // Offset with output neurons within params / types - auto pos = nOutputs_ + outPos; - neuronHelper(¶ms_[pos * MAX_NEURON_PARAMS], &types_[pos], neuronMap[neuronId]); - outPos++; - } - - // Decode connections - nNonInputs_ = nOutputs_ + nHidden_; - auto connection = node->HasElement("rv:neural_connection") ? node->GetElement("rv:neural_connection") : sdf::ElementPtr(); - while (connection) { - if (not connection->HasAttribute("src") || not connection->HasAttribute("dst") - || not connection->HasAttribute("weight")) { - std::cerr << "Missing required connection attributes (`src`, `dst` or `weight`)." << std::endl; - throw std::runtime_error("Robot brain error"); + flipState_ = !flipState_; } - auto src = connection->GetAttribute("src")->GetAsString(); - auto dst = connection->GetAttribute("dst")->GetAsString(); - double weight; - connection->GetAttribute("weight")->Get(weight); - - // Use connection helper to set the weight - connectionHelper(src, dst, weight); - - // Load the next connection - connection = connection->GetNextElement("rv:neural_connection"); - } -} - -NeuralNetwork::~NeuralNetwork() -{} - -void NeuralNetwork::step(double time) { - unsigned int i = 0; - unsigned int j = 0; - - if (nOutputs_ == 0) { - return; - } - - double *curState, *nextState; - if (flipState_) { - curState = state2_; - nextState = state1_; - } else { - curState = state1_; - nextState = state2_; - } - - - unsigned int maxNonInputs = MAX_HIDDEN_NEURONS + MAX_OUTPUT_NEURONS; - for (i = 0; i < nNonInputs_; ++i) { - double curNeuronActivation = 0; - - // Add input neuron values - for (j = 0; j < nInputs_; ++j) { - curNeuronActivation += inputWeights_[maxNonInputs * j + i] - * input_[j]; - } + void NeuralNetwork::update(const std::vector< MotorPtr > &motors, + const std::vector< SensorPtr > &sensors, + double t, + double step) + { + boost::mutex::scoped_lock lock(networkMutex_); + + // Read sensor data and feed the neural network + unsigned int p = 0; + for (auto sensor : sensors) + { + sensor->read(&input_[p]); + p += sensor->inputs(); + } - // Add output neuron values - for (j = 0; j < nOutputs_; ++j) { - curNeuronActivation += outputWeights_[maxNonInputs * j + i] * curState[j]; - } + this->step(t); - // Add hidden neuron values - for (j = 0; j < nHidden_; ++j) { - curNeuronActivation += hiddenWeights_[maxNonInputs * j + i] * curState[nOutputs_ + j]; - } + // Since the output neurons are the first in the state + // array we can just use it to update the motors directly. + double *output = flipState_ ? &state2_[0] : &state1_[0]; - unsigned int base = MAX_NEURON_PARAMS * i; - switch (types_[i]) { - case SIGMOID: - /* params are bias, gain */ - curNeuronActivation -= params_[base]; - nextState[i] = 1.0 - / (1.0 + exp(-params_[base + 1] * - curNeuronActivation)); - break; - case SIMPLE: - /* linear, params are bias, gain */ - curNeuronActivation -= params_[base]; - nextState[i] = params_[base + 1] * - curNeuronActivation; - break; - case OSCILLATOR: { // Use the block to prevent "crosses initialization" error - /* params are period, phase offset, gain (amplitude) */ - double period = params_[base]; - double phaseOffset = params_[base + 1]; - double gain = params_[base + 2]; - - /* Value in [0, 1] */ - nextState[i] = ((sin( (2.0*M_PI/period) * - (time - period * phaseOffset))) + 1.0) / 2.0; - - /* set output to be in [0.5 - gain/2, 0.5 + gain/2] */ - nextState[i] = (0.5 - (gain/2.0) + - nextState[i] * gain); - } break; - default: - // Unsupported type should never happen - std::cerr << "Invalid neuron type during processing, must be a bug." << std::endl; - throw std::runtime_error("Robot brain error"); + // Send new signals to the motors + p = 0; + for (auto motor: motors) + { + motor->update(&output[p], step); + p += motor->outputs(); + } } - } - - flipState_ = !flipState_; -} - -void NeuralNetwork::update(const std::vector& motors, - const std::vector& sensors, - double t, double step) { - boost::mutex::scoped_lock lock(networkMutex_); - - // Read sensor data and feed the neural network - unsigned int p = 0; - for (auto sensor : sensors) { - sensor->read(&input_[p]); - p += sensor->inputs(); - } - - this->step(t); - - // Since the output neurons are the first in the state - // array we can just use it to update the motors directly. - double * output = flipState_ ? &state2_[0] : &state1_[0]; - - // Send new signals to the motors - p = 0; - for (auto motor: motors) { - motor->update(&output[p], step); - p += motor->outputs(); - } -} ////////////////////////////////////////////////////////// -void NeuralNetwork::modify(ConstModifyNeuralNetworkPtr &req) { - boost::mutex::scoped_lock lock(networkMutex_); - - unsigned int i, j; - for (i = 0; i < (unsigned int)req->remove_hidden_size(); ++i) { - // Find the neuron + position - auto id = req->remove_hidden(i); - if (not positionMap_.count(id)) { - std::cerr << "Unknown neuron ID `" << id << "`" << std::endl; - throw std::runtime_error("Robot brain error"); - } - - if ("hidden" != layerMap_[id]) { - std::cerr << "Cannot remove neuron ID `" << id << "`, it is not a hidden neuron." << std::endl; - throw std::runtime_error("Robot brain error"); - } - - auto pos = positionMap_[id]; - positionMap_.erase(id); - layerMap_.erase(id); - - // Shift types - auto s = sizeof(types_[0]); - std::memmove( - // Position shifted one type to the left - types_ + (pos + nOutputs_) * s, - - // Position of next neuron type - types_ + (pos + nOutputs_ + 1) * s, - - // # of hidden neurons beyond this one - s * (nHidden_ - pos - 1) - ); - - // Shift parameters - s = sizeof(params_[0]); - memmove( - // Position of item to remove - params_ + (pos + nOutputs_) * MAX_NEURON_PARAMS * s, - - // Position of next neuron type - params_ + (pos + nOutputs_ + 1) * MAX_NEURON_PARAMS * s, - - // # of hidden neurons beyond this one - (nHidden_ - pos - 1) * MAX_NEURON_PARAMS * s - ); - - // Reposition items in weight arrays. We start with the weights - // of connections pointing *to* the neuron to be removed. - // For each entry in each of the three weights arrays we have to - // move all hidden connection weights down, then zero out the last entry - s = sizeof(inputWeights_[0]); - double* weightArrays[] = {inputWeights_, outputWeights_, hiddenWeights_}; - unsigned int sizes[] = {nInputs_, nOutputs_, nHidden_}; - - for (unsigned int k = 0; k < 3; ++k) { - auto * weights = weightArrays[k]; - auto size = sizes[k]; - - for (j = 0; j < size; ++j) { - memmove( - // Position of item to remove - weights + (nOutputs_ + pos) * s, - - // Position of next item - weights + (nOutputs_ + pos + 1) * s, - - // # of possible hidden neurons beyond this one - (MAX_HIDDEN_NEURONS - pos - 1) * s - ); - - // Zero out the last item in case a connection that corresponds - // to it is ever added. - weights[nOutputs_ + pos] = 0; + void NeuralNetwork::modify(ConstModifyNeuralNetworkPtr &req) + { + boost::mutex::scoped_lock lock(networkMutex_); + + unsigned int i, j; + for (i = 0; i < (unsigned int)req->remove_hidden_size(); ++i) + { + // Find the neuron + position + auto id = req->remove_hidden(i); + if (not positionMap_.count(id)) + { + std::cerr << "Unknown neuron ID `" << id << "`" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + if ("hidden" != layerMap_[id]) + { + std::cerr + << "Cannot remove neuron ID `" + << id + << "`, it is not a hidden neuron." + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto pos = positionMap_[id]; + positionMap_.erase(id); + layerMap_.erase(id); + + // Shift types + auto s = sizeof(types_[0]); + std::memmove( + // Position shifted one type to the left + types_ + (pos + nOutputs_) * s, + + // Position of next neuron type + types_ + (pos + nOutputs_ + 1) * s, + + // # of hidden neurons beyond this one + s * (nHidden_ - pos - 1)); + + // Shift parameters + s = sizeof(params_[0]); + std::memmove( + // Position of item to remove + params_ + (pos + nOutputs_) * MAX_NEURON_PARAMS * s, + + // Position of next neuron type + params_ + (pos + nOutputs_ + 1) * MAX_NEURON_PARAMS * s, + + // # of hidden neurons beyond this one + (nHidden_ - pos - 1) * MAX_NEURON_PARAMS * s); + + // Reposition items in weight arrays. We start with the weights + // of connections pointing *to* the neuron to be removed. + // For each entry in each of the three weights arrays we have to + // move all hidden connection weights down, then zero out the last entry + s = sizeof(inputWeights_[0]); + double *weightArrays[] = + {inputWeights_, outputWeights_, hiddenWeights_}; + unsigned int sizes[] = {nInputs_, nOutputs_, nHidden_}; + + for (unsigned int k = 0; k < 3; ++k) + { + auto *weights = weightArrays[k]; + auto size = sizes[k]; + + for (j = 0; j < size; ++j) + { + memmove( + // Position of item to remove + weights + (nOutputs_ + pos) * s, + + // Position of next item + weights + (nOutputs_ + pos + 1) * s, + + // # of possible hidden neurons beyond this one + (MAX_HIDDEN_NEURONS - pos - 1) * s); + + // Zero out the last item in case a connection that corresponds + // to it is ever added. + weights[nOutputs_ + pos] = 0; + } + } + + // Now the weights where the removed neuron is the source + // The block of weights corresponding to the neuron that is being + // removed needs to be removed by shifting down all items beyond it. + std::memmove( + // Position of the item to remove + hiddenWeights_ + pos * MAX_NON_INPUT_NEURONS * s, + + // Position of the next item + hiddenWeights_ + (pos + 1) * MAX_NON_INPUT_NEURONS * s, + + // Remaining number of memory items + (MAX_HIDDEN_NEURONS - pos - 1) * MAX_NON_INPUT_NEURONS * s); + + // Zero the remaining entries at the end + std::memset(hiddenWeights_ + (MAX_HIDDEN_NEURONS - 1) * s, + 0, + MAX_NON_INPUT_NEURONS * s); + + // Decrement the entry in the `positionMap` for all + // hidden neurons above this one. + for (auto iter = + positionMap_.begin(); iter != positionMap_.end(); ++iter) + { + auto layer = layerMap_[iter->first]; + if ("hidden" == layer && positionMap_[iter->first] > pos) + { + positionMap_[iter->first]--; + } + } + + nHidden_--; + nNonInputs_--; } - } - // Now the weights where the removed neuron is the source - // The block of weights corresponding to the neuron that is being - // removed needs to be removed by shifting down all items beyond it. - memmove( - // Position of the item to remove - hiddenWeights_ + pos * MAX_NON_INPUT_NEURONS * s, - - // Position of the next item - hiddenWeights_ + (pos + 1) * MAX_NON_INPUT_NEURONS * s, - - // Remaining number of memory items - (MAX_HIDDEN_NEURONS - pos - 1) * MAX_NON_INPUT_NEURONS * s - ); - - // Zero the remaining entries at the end - memset(hiddenWeights_ + (MAX_HIDDEN_NEURONS - 1) * s, 0, MAX_NON_INPUT_NEURONS * s); - - // Decrement the entry in the `positionMap` for all - // hidden neurons above this one. - for (auto iter = positionMap_.begin(); iter != positionMap_.end(); ++iter) { - auto layer = layerMap_[iter->first]; - if ("hidden" == layer && positionMap_[iter->first] > pos) { - positionMap_[iter->first]--; + // Add new requested hidden neurons + for (i = 0; i < (unsigned int)req->add_hidden_size(); ++i) + { + if (nHidden_ >= MAX_HIDDEN_NEURONS) + { + std::cerr + << "Cannot add hidden neuron; the max (" + << MAX_HIDDEN_NEURONS + << ") is already reached." + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto neuron = req->add_hidden(i); + auto id = neuron.id(); + if (layerMap_.count(id)) + { + std::cerr << "Adding duplicate neuron ID `" << id << "`" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + positionMap_[id] = nHidden_; + layerMap_[id] = "hidden"; + + unsigned int pos = nOutputs_ + nHidden_; + neuronHelper(¶ms_[pos * MAX_NEURON_PARAMS], &types_[pos], neuron); + nHidden_++; + nNonInputs_++; } - } - - nHidden_--; - nNonInputs_--; - } - // Add new requested hidden neurons - for (i = 0; i < (unsigned int)req->add_hidden_size(); ++i) { - if (nHidden_ >= MAX_HIDDEN_NEURONS) { - std::cerr << "Cannot add hidden neuron; the max (" - << MAX_HIDDEN_NEURONS << ") is already reached." << std::endl; - throw std::runtime_error("Robot brain error"); - } + // Update parameters of existing neurons + for (i = 0; i < (unsigned int)req->set_parameters_size(); ++i) + { + auto neuron = req->set_parameters(i); + auto id = neuron.id(); + if (not positionMap_.count(id)) + { + std::cerr << "Unknown neuron ID `" << id << "`" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto pos = positionMap_[id]; + auto layer = layerMap_[id]; + + if ("input" == layer) + { + std::cerr << "Input neurons cannot be modified." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + neuronHelper(¶ms_[pos * MAX_NEURON_PARAMS], &types_[pos], neuron); + } - auto neuron = req->add_hidden(i); - auto id = neuron.id(); - if (layerMap_.count(id)) { - std::cerr << "Adding duplicate neuron ID `" << id << "`" << std::endl; - throw std::runtime_error("Robot brain error"); + // Set weights of new or existing connections + for (i = 0; i < (unsigned int)req->set_weights_size(); ++i) + { + auto conn = req->set_weights(i); + auto src = conn.src(); + auto dst = conn.dst(); + this->connectionHelper(src, dst, conn.weight()); + } } - positionMap_[id] = nHidden_; - layerMap_[id] = "hidden"; - - unsigned int pos = nOutputs_ + nHidden_; - neuronHelper(¶ms_[pos * MAX_NEURON_PARAMS], &types_[pos], neuron); - nHidden_++; - nNonInputs_++; - } - - // Update parameters of existing neurons - for (i = 0; i < (unsigned int)req->set_parameters_size(); ++i) { - auto neuron = req->set_parameters(i); - auto id = neuron.id(); - if (not positionMap_.count(id)) { - std::cerr << "Unknown neuron ID `" << id << "`" << std::endl; - throw std::runtime_error("Robot brain error"); - } +////////////////////////////////////// - auto pos = positionMap_[id]; - auto layer = layerMap_[id]; + void NeuralNetwork::connectionHelper(const std::string &src, + const std::string &dst, + double weight) + { + if (not layerMap_.count(src)) + { + std::cerr << "Source neuron '" << src << "' is unknown." << std::endl; + throw std::runtime_error("Robot brain error"); + } - if ("input" == layer) { - std::cerr << "Input neurons cannot be modified." << std::endl; - throw std::runtime_error("Robot brain error"); - } + if (not layerMap_.count(dst)) + { + std::cerr + << "Destination neuron '" + << dst + << "' is unknown." + << std::endl; + throw std::runtime_error("Robot brain error"); + } - neuronHelper(¶ms_[pos * MAX_NEURON_PARAMS], &types_[pos], neuron); - } + auto srcLayer = layerMap_[src]; + auto dstLayer = layerMap_[dst]; - // Set weights of new or existing connections - for (i = 0; i < (unsigned int)req->set_weights_size(); ++i) { - auto conn = req->set_weights(i); - auto src = conn.src(); - auto dst = conn.dst(); - this->connectionHelper(src, dst, conn.weight()); - } -} + unsigned int srcNeuronPos = positionMap_[src], + dstNeuronPos = positionMap_[dst]; -////////////////////////////////////// + if ("input" == dstLayer) + { + std::cerr + << "Destination neuron '" + << dst + << "' is an input neuron." + << std::endl; + throw std::runtime_error("Robot brain error"); + } + else if ("hidden" == dstLayer) + { + // Offset with output neurons for hidden neuron position + dstNeuronPos += nOutputs_; + } -void NeuralNetwork::connectionHelper(const std::string & src, const std::string & dst, double weight) { - if (not layerMap_.count(src)) { - std::cerr << "Source neuron '" << src << "' is unknown." << std::endl; - throw std::runtime_error("Robot brain error"); - } - - if (not layerMap_.count(dst)) { - std::cerr << "Destination neuron '" << dst << "' is unknown." << std::endl; - throw std::runtime_error("Robot brain error"); - } - - auto srcLayer = layerMap_[src]; - auto dstLayer = layerMap_[dst]; - - unsigned int srcNeuronPos = positionMap_[src], - dstNeuronPos = positionMap_[dst]; - - if ("input" == dstLayer) { - std::cerr << "Destination neuron '" << dst << "' is an input neuron." << std::endl; - throw std::runtime_error("Robot brain error"); - } else if ("hidden" == dstLayer) { - // Offset with output neurons for hidden neuron position - dstNeuronPos += nOutputs_; - } - - // Determine the index of the weight. - unsigned int idx = (srcNeuronPos * MAX_NON_INPUT_NEURONS) + dstNeuronPos; - if ("input" == srcLayer) { - inputWeights_[idx] = weight; - } else if ("output" == srcLayer) { - outputWeights_[idx] = weight; - } else { - hiddenWeights_[idx] = weight; - } -} + // Determine the index of the weight. + unsigned int idx = (srcNeuronPos * MAX_NON_INPUT_NEURONS) + dstNeuronPos; + if ("input" == srcLayer) + { + inputWeights_[idx] = weight; + } + else if ("output" == srcLayer) + { + outputWeights_[idx] = weight; + } + else + { + hiddenWeights_[idx] = weight; + } + } ///////////////////////////////////////////////// -void neuronHelper(double* params, unsigned int* types, sdf::ElementPtr neuron) { - if (not neuron->HasAttribute("type")) { - std::cerr << "Missing required `type` attribute for neuron." << std::endl; + void neuronHelper(double *params, + unsigned int *types, + sdf::ElementPtr neuron) + { + if (not neuron->HasAttribute("type")) + { + std::cerr + << "Missing required `type` attribute for neuron." + << std::endl; throw std::runtime_error("Robot brain error"); - } - - auto type = neuron->GetAttribute("type")->GetAsString(); - if ("Sigmoid" == type || "Simple" == type) { - types[0] = "Simple" == type ? SIMPLE : SIGMOID; - - if (not neuron->HasElement("rv:bias") || not neuron->HasElement("rv:gain")) { - std::cerr << "A `" << type << "` neuron requires `rv:bias` and `rv:gain` elements." << std::endl; - throw std::runtime_error("Robot brain error"); - } - - // Set bias and gain parameters - params[0] = neuron->GetElement("rv:bias")->Get< double >(); - params[1] = neuron->GetElement("rv:gain")->Get< double >(); - } else if ("Oscillator" == type) { - types[0] = OSCILLATOR; - - if (not neuron->HasElement("rv:period") - || not neuron->HasElement("rv:phase_offset") - || not neuron->HasElement("rv:amplitude")) { - std::cerr << "An `Oscillator` neuron requires `rv:period`" - << ", `rv:phase_offset`" - << ", and `rv:amplitude` elements." << std::endl; - throw std::runtime_error("Robot brain error"); - } + } - // Set period, phase offset and gain - params[0] = neuron->GetElement("rv:period")->Get< double >(); - params[1] = neuron->GetElement("rv:phase_offset")->Get< double >(); - params[2] = neuron->GetElement("rv:amplitude")->Get< double >(); - } else { - std::cerr << "Unsupported neuron type `" << type << '`' << std::endl; - throw std::runtime_error("Robot brain error"); - } -} - -void neuronHelper(double* params, unsigned int* types, const revolve::msgs::Neuron & neuron) { - auto type = neuron.type(); - if ("Sigmoid" == type || "Simple" == type) { - types[0] = "Simple" == type ? SIMPLE : SIGMOID; - if (neuron.param_size() != 2) { - std::cerr << "A `" << type << "` neuron requires exactly two parameters." << std::endl; - throw std::runtime_error("Robot brain error"); + auto type = neuron->GetAttribute("type")->GetAsString(); + if ("Sigmoid" == type || "Simple" == type) + { + types[0] = "Simple" == type ? SIMPLE : SIGMOID; + + if (not neuron->HasElement("rv:bias") || not neuron->HasElement( + "rv:gain")) + { + std::cerr + << "A `" + << type + << "` neuron requires `rv:bias` and `rv:gain` elements." + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + // Set bias and gain parameters + params[0] = neuron->GetElement("rv:bias")->Get< double >(); + params[1] = neuron->GetElement("rv:gain")->Get< double >(); + } + else if ("Oscillator" == type) + { + types[0] = OSCILLATOR; + + if (not neuron->HasElement("rv:period") + || not neuron->HasElement("rv:phase_offset") + || not neuron->HasElement("rv:amplitude")) + { + std::cerr << "An `Oscillator` neuron requires `rv:period`" + << ", `rv:phase_offset`" + << ", and `rv:amplitude` elements." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + // Set period, phase offset and gain + params[0] = neuron->GetElement("rv:period")->Get< double >(); + params[1] = neuron->GetElement("rv:phase_offset")->Get< double >(); + params[2] = neuron->GetElement("rv:amplitude")->Get< double >(); + } + else + { + std::cerr << "Unsupported neuron type `" << type << '`' << std::endl; + throw std::runtime_error("Robot brain error"); + } } - // Set bias and gain parameters - params[0] = neuron.param(0).value(); - params[1] = neuron.param(1).value(); - } else if ("Oscillator" == type) { - types[0] = OSCILLATOR; - - if (neuron.param_size() != 3) { - std::cerr << "A `" << type << "` neuron requires exactly three parameters." << std::endl; - throw std::runtime_error("Robot brain error"); + void neuronHelper(double *params, + unsigned int *types, + const revolve::msgs::Neuron &neuron) + { + auto type = neuron.type(); + if ("Sigmoid" == type || "Simple" == type) + { + types[0] = "Simple" == type ? SIMPLE : SIGMOID; + if (neuron.param_size() != 2) + { + std::cerr + << "A `" + << type + << "` neuron requires exactly two parameters." + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + // Set bias and gain parameters + params[0] = neuron.param(0).value(); + params[1] = neuron.param(1).value(); + } + else if ("Oscillator" == type) + { + types[0] = OSCILLATOR; + + if (neuron.param_size() != 3) + { + std::cerr + << "A `" + << type + << "` neuron requires exactly three parameters." + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + params[0] = neuron.param(0).value(); + params[1] = neuron.param(1).value(); + params[2] = neuron.param(2).value(); + } + else + { + std::cerr << "Unsupported neuron type `" << type << '`' << std::endl; + throw std::runtime_error("Robot brain error"); + } } - - params[0] = neuron.param(0).value(); - params[1] = neuron.param(1).value(); - params[2] = neuron.param(2).value(); - } else { - std::cerr << "Unsupported neuron type `" << type << '`' << std::endl; - throw std::runtime_error("Robot brain error"); - } -} - -} /* namespace gazebo */ -} /* namespace revolve */ + } /* namespace gazebo */ +} /* namespace revolve */ diff --git a/cpp/revolve/gazebo/brain/NeuralNetwork.h b/cpp/revolve/gazebo/brain/NeuralNetwork.h index f09534734a..8c0da19bac 100644 --- a/cpp/revolve/gazebo/brain/NeuralNetwork.h +++ b/cpp/revolve/gazebo/brain/NeuralNetwork.h @@ -50,151 +50,161 @@ // (bias, tau, gain) or (phase offset, period, gain) #define MAX_NEURON_PARAMS 3 -namespace revolve { -namespace gazebo { - -typedef const boost::shared_ptr ConstModifyNeuralNetworkPtr; +namespace revolve +{ + namespace gazebo + { + typedef const boost::shared_ptr< revolve::msgs::ModifyNeuralNetwork const > + ConstModifyNeuralNetworkPtr; /* * Copied from NeuronRepresentation.h */ -enum neuronType -{ - INPUT, - SIMPLE, - SIGMOID, - CTRNN_SIGMOID, - OSCILLATOR, - SUPG -}; - -class NeuralNetwork: public Brain { - public: - /** - * @param Name of the robot - * @param The brain node - * @param Reference to motor list, which might be reordered - * @param Reference to the sensor list, which might be reordered - */ - NeuralNetwork(std::string modelName, - sdf::ElementPtr node, - std::vector< MotorPtr > & motors, - std::vector< SensorPtr > & sensors); - virtual ~NeuralNetwork(); - - /** - * @param Motor list - * @param Sensor list - */ - virtual void update(const std::vector< MotorPtr > & motors, - const std::vector< SensorPtr > & sensors, - double t, - double step); - - protected: - /** - * Steps the neural network - */ - void step(double time); - - /** - * Request handler to modify the neural network - */ - void modify(ConstModifyNeuralNetworkPtr &req); - - // Mutex for stepping / updating the network - boost::mutex networkMutex_; - - /** - * Transport node - */ - ::gazebo::transport::NodePtr node_; - - /** - * Network modification subscriber - */ - ::gazebo::transport::SubscriberPtr alterSub_; - - /* - * Connection weights, separated into three arrays for convenience. Note - * that only output and hidden neurons are weight targets. - * - * Weights are stored with gaps, meaning that every neuron holds entries for - * the maximum possible number of connections. This makes restructuring the - * weights arrays when a hidden neuron is removed slightly less cumbersome. - */ - double inputWeights_[MAX_INPUT_NEURONS * (MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; - double outputWeights_[MAX_OUTPUT_NEURONS * (MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; - double hiddenWeights_[MAX_HIDDEN_NEURONS * (MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; - - // Unlike weights, types, params and current states are stored without - // gaps, meaning the first `m` entries are for output neurons, followed - // by `n` entries for hidden neurons. If a hidden neuron is removed, - // the items beyond it are moved back. - /** - * Type of each non-input neuron - */ - unsigned int types_[(MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; - - /* - * Params for hidden and output neurons, quantity depends on the type of - * neuron - */ - double params_[MAX_NEURON_PARAMS * (MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; - - /* - * Output states arrays for the current state and the next state. - */ - double state1_[MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS]; - double state2_[MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS]; - - /** - * One input state for each input neuron - */ - double input_[MAX_INPUT_NEURONS]; - - /** - * Used to determine the current state array. False = state1, - * true = state2. - */ - bool flipState_; - - /** - * Stores the type of each neuron ID - */ - std::map layerMap_; - - /** - * Stores the position of each neuron ID, relative to its type - */ - std::map positionMap_; - - /** - * The number of inputs - */ - unsigned int nInputs_; - - /** - * The number of outputs - */ - unsigned int nOutputs_; - - /** - * The number of hidden units - */ - unsigned int nHidden_; - - /** - * The number of non-inputs (i.e. nOutputs + nHidden) - */ - unsigned int nNonInputs_; - - private: void connectionHelper(const std::string &src, - const std::string &dst, - double weight); -}; - -} /* namespace gazebo */ + enum neuronType + { + INPUT, + SIMPLE, + SIGMOID, + CTRNN_SIGMOID, + OSCILLATOR, + SUPG + }; + + class NeuralNetwork + : public Brain + { + public: + /** + * @param Name of the robot + * @param The brain node + * @param Reference to motor list, which might be reordered + * @param Reference to the sensor list, which might be reordered + */ + NeuralNetwork(std::string modelName, + sdf::ElementPtr node, + std::vector< MotorPtr > &motors, + std::vector< SensorPtr > &sensors); + + virtual ~NeuralNetwork(); + + /** + * @param Motor list + * @param Sensor list + */ + virtual void update(const std::vector< MotorPtr > &motors, + const std::vector< SensorPtr > &sensors, + double t, + double step); + + protected: + /** + * Steps the neural network + */ + void step(double time); + + /** + * Request handler to modify the neural network + */ + void modify(ConstModifyNeuralNetworkPtr &req); + + // Mutex for stepping / updating the network + boost::mutex networkMutex_; + + /** + * Transport node + */ + ::gazebo::transport::NodePtr node_; + + /** + * Network modification subscriber + */ + ::gazebo::transport::SubscriberPtr alterSub_; + + /* + * Connection weights, separated into three arrays for convenience. Note + * that only output and hidden neurons are weight targets. + * + * Weights are stored with gaps, meaning that every neuron holds entries + * for the maximum possible number of connections. This makes + * restructuring the weights arrays when a hidden neuron is removed + * slightly less cumbersome. + */ + double inputWeights_[MAX_INPUT_NEURONS + * (MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; + double outputWeights_[MAX_OUTPUT_NEURONS + * (MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; + double hiddenWeights_[MAX_HIDDEN_NEURONS + * (MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; + + // Unlike weights, types, params and current states are stored without + // gaps, meaning the first `m` entries are for output neurons, followed + // by `n` entries for hidden neurons. If a hidden neuron is removed, + // the items beyond it are moved back. + /** + * Type of each non-input neuron + */ + unsigned int types_[(MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; + + /* + * Params for hidden and output neurons, quantity depends on the type of + * neuron + */ + double params_[ + MAX_NEURON_PARAMS * (MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS)]; + + /* + * Output states arrays for the current state and the next state. + */ + double state1_[MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS]; + double state2_[MAX_OUTPUT_NEURONS + MAX_HIDDEN_NEURONS]; + + /** + * One input state for each input neuron + */ + double input_[MAX_INPUT_NEURONS]; + + /** + * Used to determine the current state array. False = state1, + * true = state2. + */ + bool flipState_; + + /** + * Stores the type of each neuron ID + */ + std::map< std::string, std::string > layerMap_; + + /** + * Stores the position of each neuron ID, relative to its type + */ + std::map< std::string, unsigned int > positionMap_; + + /** + * The number of inputs + */ + unsigned int nInputs_; + + /** + * The number of outputs + */ + unsigned int nOutputs_; + + /** + * The number of hidden units + */ + unsigned int nHidden_; + + /** + * The number of non-inputs (i.e. nOutputs + nHidden) + */ + unsigned int nNonInputs_; + + private: + void connectionHelper(const std::string &src, + const std::string &dst, + double weight); + }; + } /* namespace gazebo */ } /* namespace revolve */ #endif /* REVOLVE_GAZEBO_BRAIN_NEURALNETWORK_H_ */ diff --git a/cpp/revolve/gazebo/motors/JointMotor.cpp b/cpp/revolve/gazebo/motors/JointMotor.cpp index 305371f8d4..52dae5e4c7 100644 --- a/cpp/revolve/gazebo/motors/JointMotor.cpp +++ b/cpp/revolve/gazebo/motors/JointMotor.cpp @@ -17,36 +17,42 @@ * */ +#include + #include namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -JointMotor::JointMotor(gz::physics::ModelPtr model, - std::string partId, - std::string motorId, - sdf::ElementPtr motor, - unsigned int outputs) - : Motor(model, partId, motorId, outputs) +namespace revolve { - if (not motor->HasAttribute("joint")) { - std::cerr << "JointMotor requires a `joint` attribute." << std::endl; - throw std::runtime_error("Motor error"); - } - - auto jointName = motor->GetAttribute("joint")->GetAsString(); - joint_ = model->GetJoint(jointName); - if (not joint_) { - std::cerr << "Cannot locate joint motor `" << jointName << "`" << std::endl; - throw std::runtime_error("Motor error"); - } - - jointName_ = joint_->GetScopedName(); -} - -JointMotor::~JointMotor() {} - -} /* namespace gazebo */ -} /* namespace revolve */ + namespace gazebo + { + JointMotor::JointMotor(gz::physics::ModelPtr model, + std::string partId, + std::string motorId, + sdf::ElementPtr motor, + unsigned int outputs) + : Motor(model, partId, motorId, outputs) + { + if (not motor->HasAttribute("joint")) + { + std::cerr << "JointMotor requires a `joint` attribute." << std::endl; + throw std::runtime_error("Motor error"); + } + + auto jointName = motor->GetAttribute("joint")->GetAsString(); + joint_ = model->GetJoint(jointName); + if (not joint_) + { + std::cerr << "Cannot locate joint motor `" << jointName + << "`" << std::endl; + throw std::runtime_error("Motor error"); + } + + jointName_ = joint_->GetScopedName(); + } + + JointMotor::~JointMotor() + {} + } /* namespace gazebo */ +} /* namespace revolve */ diff --git a/cpp/revolve/gazebo/motors/Motor.cpp b/cpp/revolve/gazebo/motors/Motor.cpp index 97e5e6640b..7d2df19ba3 100644 --- a/cpp/revolve/gazebo/motors/Motor.cpp +++ b/cpp/revolve/gazebo/motors/Motor.cpp @@ -17,48 +17,79 @@ * */ +#include + #include namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -Motor::Motor(::gazebo::physics::ModelPtr model, std::string partId, std::string motorId, unsigned int outputNeurons): - model_(model), - partId_(partId), - motorId_(motorId), - outputs_(outputNeurons) -{} - -Motor::~Motor() {} +namespace revolve +{ + namespace gazebo + { + Motor::Motor(::gazebo::physics::ModelPtr model, + std::string partId, + std::string motorId, + unsigned int outputNeurons) + :model_(model) + , partId_(partId) + , motorId_(motorId) + , outputs_(outputNeurons) + {} -std::string Motor::partId() { - return partId_; -} + Motor::~Motor() + {} -unsigned int Motor::outputs() { - return outputs_; -} + std::string Motor::partId() + { + return partId_; + } -gz::common::PID Motor::createPid(sdf::ElementPtr pidElem) { - double pv = 0, iv = 0, dv = 0, iMax = 0, iMin = 0, - cmdMax = 0, cmdMin = 0; + unsigned int Motor::outputs() + { + return outputs_; + } - if (pidElem->HasElement("rv:p")) pv = pidElem->GetElement("rv:p")->Get(); - if (pidElem->HasElement("rv:i")) iv = pidElem->GetElement("rv:i")->Get(); - if (pidElem->HasElement("rv:d")) dv = pidElem->GetElement("rv:d")->Get(); - if (pidElem->HasElement("rv:i_max")) iMax = pidElem->GetElement("rv:i_max")->Get(); - if (pidElem->HasElement("rv:i_min")) iMin = pidElem->GetElement("rv:i_min")->Get(); - if (pidElem->HasElement("rv:cmd_max")) cmdMax = pidElem->GetElement("rv:cmd_max")->Get(); - if (pidElem->HasElement("rv:cmd_min")) cmdMin = pidElem->GetElement("rv:cmd_min")->Get(); + gz::common::PID Motor::createPid(sdf::ElementPtr pidElem) + { + double pv = 0, iv = 0, dv = 0, iMax = 0, iMin = 0, + cmdMax = 0, cmdMin = 0; - return gz::common::PID(pv, iv, dv, iMax, iMin, cmdMax, cmdMin); -} + if (pidElem->HasElement("rv:p")) + { + pv = pidElem->GetElement("rv:p")->Get< double >(); + } + if (pidElem->HasElement("rv:i")) + { + iv = pidElem->GetElement("rv:i")->Get< double >(); + } + if (pidElem->HasElement("rv:d")) + { + dv = pidElem->GetElement("rv:d")->Get< double >(); + } + if (pidElem->HasElement("rv:i_max")) + { + iMax = pidElem->GetElement("rv:i_max")->Get< double >(); + } + if (pidElem->HasElement("rv:i_min")) + { + iMin = pidElem->GetElement("rv:i_min")->Get< double >(); + } + if (pidElem->HasElement("rv:cmd_max")) + { + cmdMax = pidElem->GetElement("rv:cmd_max")->Get< double >(); + } + if (pidElem->HasElement("rv:cmd_min")) + { + cmdMin = pidElem->GetElement("rv:cmd_min")->Get< double >(); + } -std::string Motor::motorId() { - return motorId_; -} + return gz::common::PID(pv, iv, dv, iMax, iMin, cmdMax, cmdMin); + } -} /* namespace gazebo */ -} /* namespace revolve */ + std::string Motor::motorId() + { + return motorId_; + } + } /* namespace gazebo */ +} /* namespace revolve */ diff --git a/cpp/revolve/gazebo/motors/MotorFactory.cpp b/cpp/revolve/gazebo/motors/MotorFactory.cpp index d0d62ec141..ce4a0d3098 100644 --- a/cpp/revolve/gazebo/motors/MotorFactory.cpp +++ b/cpp/revolve/gazebo/motors/MotorFactory.cpp @@ -17,57 +17,69 @@ * */ +#include +#include + #include #include -#include - namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -MotorFactory::MotorFactory(::gazebo::physics::ModelPtr model): - model_(model) -{} - -MotorFactory::~MotorFactory() {} +namespace revolve +{ + namespace gazebo + { + MotorFactory::MotorFactory(::gazebo::physics::ModelPtr model) : + model_(model) + {} -MotorPtr MotorFactory::getMotor(sdf::ElementPtr motor, const std::string & type, - const std::string & partId, const std::string & motorId) { - MotorPtr motorObj; - if ("position" == type) { - motorObj.reset(new PositionMotor(model_, partId, motorId, motor)); - } else if ("velocity" == type) { - motorObj.reset(new VelocityMotor(model_, partId, motorId, motor)); - } + MotorFactory::~MotorFactory() + {} - return motorObj; -} + MotorPtr MotorFactory::getMotor(sdf::ElementPtr motor, + const std::string &type, + const std::string &partId, + const std::string &motorId) + { + MotorPtr motorObj; + if ("position" == type) + { + motorObj.reset(new PositionMotor(model_, partId, motorId, motor)); + } + else if ("velocity" == type) + { + motorObj.reset(new VelocityMotor(model_, partId, motorId, motor)); + } -MotorPtr MotorFactory::create(sdf::ElementPtr motor) { - auto typeParam = motor->GetAttribute("type"); - auto partIdParam = motor->GetAttribute("part_id"); - auto idParam = motor->GetAttribute("id"); + return motorObj; + } - if (not typeParam || not partIdParam || not idParam) { - std::cerr << "Motor is missing required attributes (`id`, `type` or `part_id`)." << std::endl; - throw std::runtime_error("Motor error"); - } + MotorPtr MotorFactory::create(sdf::ElementPtr motor) + { + auto typeParam = motor->GetAttribute("type"); + auto partIdParam = motor->GetAttribute("part_id"); + auto idParam = motor->GetAttribute("id"); - auto partId = partIdParam->GetAsString(); - auto type = typeParam->GetAsString(); - auto id = idParam->GetAsString(); - MotorPtr motorObj = this->getMotor(motor, type, partId, id); + if (not typeParam || not partIdParam || not idParam) + { + std::cerr << "Motor is missing required attributes " + "(`id`, `type` or `part_id`)." << std::endl; + throw std::runtime_error("Motor error"); + } - if (not motorObj) { - std::cerr << "Motor type '" << type << - "' is unknown." << std::endl; - throw std::runtime_error("Motor error"); - } + auto partId = partIdParam->GetAsString(); + auto type = typeParam->GetAsString(); + auto id = idParam->GetAsString(); + MotorPtr motorObj = this->getMotor(motor, type, partId, id); - return motorObj; -} + if (not motorObj) + { + std::cerr << "Motor type '" << type << + "' is unknown." << std::endl; + throw std::runtime_error("Motor error"); + } -} /* namespace gazebo */ -} /* namespace revolve */ + return motorObj; + } + } /* namespace gazebo */ +} /* namespace revolve */ diff --git a/cpp/revolve/gazebo/motors/PositionMotor.cpp b/cpp/revolve/gazebo/motors/PositionMotor.cpp index 06410f9831..3976914cda 100644 --- a/cpp/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpp/revolve/gazebo/motors/PositionMotor.cpp @@ -16,103 +16,119 @@ * */ -#include "PositionMotor.h" -#include +#include + #include +#include "PositionMotor.h" + namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -PositionMotor::PositionMotor(gz::physics::ModelPtr model, std::string partId, - std::string motorId, sdf::ElementPtr motor): - JointMotor(model, partId, motorId, motor, 1), - positionTarget_(0), - noise_(0) { - // Retrieve upper / lower limit from joint set in parent constructor - // Truncate ranges to [-pi, pi] - upperLimit_ = fmin(M_PI, joint_->GetUpperLimit(0).Radian()); - lowerLimit_ = fmax(-M_PI, joint_->GetLowerLimit(0).Radian()); - fullRange_ = (upperLimit_ - lowerLimit_ + 1e-12) >= (2 * M_PI); - - if (motor->HasElement("rv:pid")) { - auto pidElem = motor->GetElement("rv:pid"); - pid_ = Motor::createPid(pidElem); - } - - auto noiseParam = motor->GetAttribute("noise"); - if (noiseParam) { - noiseParam->Get(noise_); - } - - // I've asked this question at the Gazebo forums: - // http://answers.gazebosim.org/question/9071/joint-target-velocity-with-maximum-force/ - // Until it is answered I'm resorting to calling ODE functions directly to get this - // to work. This will result in some deprecation warnings. - // It has the added benefit of not requiring the world update connection though. +namespace revolve +{ + namespace gazebo + { + PositionMotor::PositionMotor(gz::physics::ModelPtr model, + std::string partId, + std::string motorId, + sdf::ElementPtr motor) : + JointMotor(model, partId, motorId, motor, 1), positionTarget_(0) + , noise_(0) + { + // Retrieve upper / lower limit from joint set in parent constructor + // Truncate ranges to [-pi, pi] + upperLimit_ = fmin(M_PI, joint_->GetUpperLimit(0).Radian()); + lowerLimit_ = fmax(-M_PI, joint_->GetLowerLimit(0).Radian()); + fullRange_ = (upperLimit_ - lowerLimit_ + 1e-12) >= (2 * M_PI); + + if (motor->HasElement("rv:pid")) + { + auto pidElem = motor->GetElement("rv:pid"); + pid_ = Motor::createPid(pidElem); + } + + auto noiseParam = motor->GetAttribute("noise"); + if (noiseParam) + { + noiseParam->Get(noise_); + } + + // I've asked this question at the Gazebo forums: + // http://answers.gazebosim.org/question/9071/ + // joint-target-velocity-with-maximum-force/ + // Until it is answered I'm resorting to calling ODE functions directly to + // get this + // to work. This will result in some deprecation warnings. + // It has the added benefit of not requiring the world update connection + // though. + // updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin(boost::bind( // &PositionMotor::OnUpdate, this, _1)); - double maxEffort = joint_->GetEffortLimit(0); - joint_->SetParam("fmax", 0, maxEffort); -} + double maxEffort = joint_->GetEffortLimit(0); + joint_->SetParam("fmax", 0, maxEffort); + } -PositionMotor::~PositionMotor() { } + PositionMotor::~PositionMotor() + {} -//void PositionMotor::OnUpdate(const ::gazebo::common::UpdateInfo info) { +// void PositionMotor::OnUpdate(const ::gazebo::common::UpdateInfo info) { // DoUpdate(info.simTime); -//} - -void PositionMotor::update(double *outputs, double /*step*/) { - // Just one network output, which is the first - double output = outputs[0]; - - // Motor noise in range +/- noiseLevel * actualValue - output += ((2 * gz::math::Rand::GetDblUniform() * noise_) - - noise_) * output; - - // Truncate output to [0, 1] - // HACK Don't actually target the full joint range, this way - // a low update rate won't mess with the joint constraints as much leading - // to a more stable system. - output = fmin(fmax(1e-5, output), 0.99999); - positionTarget_ = lowerLimit_ + output * (upperLimit_ - lowerLimit_); - - // Perform the actual motor update - this->DoUpdate(joint_->GetWorld()->GetSimTime()); -} - -void PositionMotor::DoUpdate(const ::gazebo::common::Time &simTime) { - gz::common::Time stepTime = simTime - prevUpdateTime_; - if (stepTime <= 0) { - // Only respond to positive step times - return; - } - - prevUpdateTime_ = simTime; - auto positionAngle = joint_->GetAngle(0); - - // TODO Make sure normalized angle lies within possible range - // I get the feeling we might be moving motors outside their - // allowed range. Also something to be aware of when setting - // the direction. - positionAngle.Normalize(); - double position = positionAngle.Radian(); - - if (fullRange_ && fabs(position - positionTarget_) > M_PI) { - // Both the position and the position target will be in the range [-pi, pi] - // For a full range of motion joint, using an angle +- 2 PI might result - // in a much shorter required movement. In this case we best correct the - // current position to something outside the range. - position += (position > 0 ? -2 * M_PI : 2 * M_PI); - } - - double error = position - positionTarget_; - double cmd = pid_.Update(error, stepTime); - - joint_->SetParam("vel", 0, cmd); -} - -} /* namespace gazebo */ -} /* namespace revolve */ +// } + void PositionMotor::update(double *outputs, + double /*step*/) + { + // Just one network output, which is the first + double output = outputs[0]; + + // Motor noise in range +/- noiseLevel * actualValue + output += ((2 * gz::math::Rand::GetDblUniform() * noise_) - + noise_) * output; + + // Truncate output to [0, 1] + // HACK Don't actually target the full joint range, this way + // a low update rate won't mess with the joint constraints as much leading + // to a more stable system. + output = fmin(fmax(1e-5, output), 0.99999); + positionTarget_ = lowerLimit_ + output * (upperLimit_ - lowerLimit_); + + // Perform the actual motor update + this->DoUpdate(joint_->GetWorld()->GetSimTime()); + } + + void PositionMotor::DoUpdate(const ::gazebo::common::Time &simTime) + { + gz::common::Time stepTime = simTime - prevUpdateTime_; + if (stepTime <= 0) + { + // Only respond to positive step times + return; + } + + prevUpdateTime_ = simTime; + auto positionAngle = joint_->GetAngle(0); + + // TODO Make sure normalized angle lies within possible range + // I get the feeling we might be moving motors outside their + // allowed range. Also something to be aware of when setting + // the direction. + positionAngle.Normalize(); + double position = positionAngle.Radian(); + + if (fullRange_ && fabs(position - positionTarget_) > M_PI) + { + // Both the position and the position target will be in the range + // [-pi, pi] + // For a full range of motion joint, using an angle +- 2 PI might result + // in a much shorter required movement. In this case we best correct the + // current position to something outside the range. + position += (position > 0 ? -2 * M_PI : 2 * M_PI); + } + + double error = position - positionTarget_; + double cmd = pid_.Update(error, stepTime); + + joint_->SetParam("vel", 0, cmd); + } + } /* namespace gazebo */ +} /* namespace revolve */ diff --git a/cpp/revolve/gazebo/motors/VelocityMotor.cpp b/cpp/revolve/gazebo/motors/VelocityMotor.cpp index 91a53d8e5e..22bf44902c 100644 --- a/cpp/revolve/gazebo/motors/VelocityMotor.cpp +++ b/cpp/revolve/gazebo/motors/VelocityMotor.cpp @@ -16,68 +16,80 @@ * */ +#include + #include "VelocityMotor.h" namespace gz = gazebo; -namespace revolve { -namespace gazebo { - - -VelocityMotor::VelocityMotor(::gazebo::physics::ModelPtr model, - std::string partId, - std::string motorId, - sdf::ElementPtr motor) - : JointMotor(model, partId, motorId, motor, 1) - , velocityTarget_(0) - , noise_(0) +namespace revolve { - if (motor->HasElement("rv:pid")) { - auto pidElem = motor->GetElement("rv:pid"); - pid_ = Motor::createPid(pidElem); - } + namespace gazebo + { + VelocityMotor::VelocityMotor(::gazebo::physics::ModelPtr model, + std::string partId, + std::string motorId, + sdf::ElementPtr motor) + : JointMotor(model, partId, motorId, motor, 1) + , velocityTarget_(0) + , noise_(0) + { + if (motor->HasElement("rv:pid")) + { + auto pidElem = motor->GetElement("rv:pid"); + pid_ = Motor::createPid(pidElem); + } - if (not motor->HasAttribute("min_velocity") || not motor->HasAttribute("max_velocity")) { + if (not motor->HasAttribute("min_velocity") + || not motor->HasAttribute("max_velocity")) + { std::cerr << "Missing servo min/max velocity parameters, " "velocity will be zero." << std::endl; minVelocity_ = maxVelocity_ = 0; - } else { + } + else + { motor->GetAttribute("min_velocity")->Get(minVelocity_); motor->GetAttribute("max_velocity")->Get(maxVelocity_); - } + } - // I've asked this question at the Gazebo forums: - // http://answers.gazebosim.org/question/9071/joint-target-velocity-with-maximum-force/ - // Until it is answered I'm resorting to calling ODE functions directly to get this - // to work. This will result in some deprecation warnings. The update connection - // is no longer needed though.; - double maxEffort = joint_->GetEffortLimit(0); - joint_->SetParam("fmax", 0, maxEffort); -} - -VelocityMotor::~VelocityMotor() {} + // I've asked this question at the Gazebo forums: + // http://answers.gazebosim.org/question/9071/ + // joint-target-velocity-with-maximum-force/ + // Until it is answered I'm resorting to calling ODE functions directly + // to get this to work. This will result in some deprecation warnings. + // The update connection is no longer needed though.; + double maxEffort = joint_->GetEffortLimit(0); + joint_->SetParam("fmax", 0, maxEffort); + } -void VelocityMotor::update(double * outputs, double /*step*/) { - // Just one network output, which is the first - double output = outputs[0]; + VelocityMotor::~VelocityMotor() + {} - // Motor noise in range +/- noiseLevel * actualValue - output += ((2 * gz::math::Rand::GetDblUniform() * noise_) - - noise_) * output; + void VelocityMotor::update(double *outputs, + double /*step*/) + { + // Just one network output, which is the first + double output = outputs[0]; - // Truncate output to [0, 1] - output = fmax(fmin(output, 1), 0); - velocityTarget_ = minVelocity_ + output * (maxVelocity_ - minVelocity_); - DoUpdate(joint_->GetWorld()->GetSimTime()); -} + // Motor noise in range +/- noiseLevel * actualValue + output += ((2 * gz::math::Rand::GetDblUniform() * noise_) - + noise_) * output; -void VelocityMotor::DoUpdate(const ::gazebo::common::Time &/*simTime*/) { - // I'm caving for now and am setting ODE parameters directly. - // See http://answers.gazebosim.org/question/9071/joint-target-velocity-with-maximum-force/ - joint_->SetParam("vel", 0, velocityTarget_); -} + // Truncate output to [0, 1] + output = fmax(fmin(output, 1), 0); + velocityTarget_ = minVelocity_ + output * (maxVelocity_ - minVelocity_); + DoUpdate(joint_->GetWorld()->GetSimTime()); + } -} // namespace gazebo -} // namespace revolve + void VelocityMotor::DoUpdate(const ::gazebo::common::Time &/*simTime*/) + { + // I'm caving for now and am setting ODE parameters directly. + // See http://answers.gazebosim.org/question/9071/ + // joint-target-velocity-with-maximum-force/ + joint_->SetParam("vel", 0, velocityTarget_); + } + } // namespace gazebo +} // namespace revolve diff --git a/cpp/revolve/gazebo/plugin/BodyAnalyzer.cpp b/cpp/revolve/gazebo/plugin/BodyAnalyzer.cpp index f7dcb466cf..1755c2e132 100644 --- a/cpp/revolve/gazebo/plugin/BodyAnalyzer.cpp +++ b/cpp/revolve/gazebo/plugin/BodyAnalyzer.cpp @@ -16,243 +16,293 @@ * */ +#include +#include + #include #include #include #include #include "BodyAnalyzer.h" -#include -#include namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -void BodyAnalyzer::Load(gz::physics::WorldPtr world, sdf::ElementPtr /*_sdf*/) { - // Store pointer to the world - world_ = world; - - // Pause the world if it is not already paused - world->SetPaused(true); - - // Set initial state values - processing_ = false; - counter_ = 0; - currentRequest_ = -1; - - // Create a new transport node for advertising / subscribing - node_.reset(new gz::transport::Node()); - node_->Init(); - - // Subscribe to the contacts message. Note that the contact manager does - // not generate data without at least one subscriber, so there is no use - // in bypassing the messaging mechanism. - contactsSub_ = node_->Subscribe("~/physics/contacts", &BodyAnalyzer::OnContacts, this); - - // Subscribe to analysis request messages - requestSub_ = node_->Subscribe("~/request", &BodyAnalyzer::AnalyzeRequest, this); - - // Since models are added asynchronously, we need some way of detecting our model add. - // We do this using a model info subscriber. - modelSub_ = node_->Subscribe("~/model/info", &BodyAnalyzer::OnModel, this); - - // Publisher for the results - responsePub_ = node_->Advertise("~/response"); - - // Subscribe to model delete events - deleteSub_ = node_->Subscribe("~/response", &BodyAnalyzer::OnModelDelete, this); - - // Hello world! - std::cout << "Body analyzer ready" << std::endl; -} - -void BodyAnalyzer::OnModel(ConstModelPtr & msg) { - std::string expectedName = "analyze_bot_"+boost::lexical_cast(counter_); - if (msg->name() != expectedName) { - throw std::runtime_error("INTERNAL ERROR: Expecting model with name '" + expectedName + - "', created model was '" + msg->name() + "'."); - } - - if (world_->GetModels().size() > 1) { - throw std::runtime_error("INTERNAL ERROR: Too many models in analyzer."); - } - - // Unpause the world so contacts will be handled - world_->SetPaused(false); -} - -void BodyAnalyzer::OnModelDelete(ConstResponsePtr & msg) { - if (msg->request() == "entity_delete") { - // This might have cleared the world - this->ProcessQueue(); - } -} - -void BodyAnalyzer::Advance() { - processingMutex_.lock(); - - // Clear current request to indicate nothing is being - // processed, and move on to the next item if there is one. - counter_++; - processing_ = false; - currentRequest_ = -1; - - // ProcessQueue needs the processing mutex, release it - processingMutex_.unlock(); - this->ProcessQueue(); -} - -void BodyAnalyzer::OnContacts(ConstContactsPtr &msg) { - // Pause the world so no new contacts will come in - world_->SetPaused(true); - - boost::mutex::scoped_lock plock(processingMutex_); - if (not processing_) { - return; - } - plock.unlock(); - - if (world_->GetModelCount() > 1) { - std::cerr << "WARNING: Too many models in the world." << std::endl; - } - - // Create a response - msgs::BodyAnalysisResponse response; - gz::msgs::Response wrapper; - wrapper.set_id(currentRequest_); - wrapper.set_request("analyze_body"); - - // Add contact info - for (auto contact : msg->contact()) { - auto msgContact = response.add_contact(); - msgContact->set_collision1(contact.collision1()); - msgContact->set_collision2(contact.collision2()); - } - - std::string name = "analyze_bot_"+boost::lexical_cast(counter_); - gz::physics::ModelPtr model = world_->GetModel(name); - - if (not model) { - std::cerr << "------------------------------------" << std::endl; - std::cerr << "INTERNAL ERROR, contact model not found: " << name << std::endl; - std::cerr << "Please retry this request." << std::endl; - std::cerr << "------------------------------------" << std::endl; - wrapper.set_response("error"); - response.SerializeToString(wrapper.mutable_serialized_data()); - responsePub_->Publish(wrapper); - - // Advance manually - this->Advance(); - return; - } - - // Add the bounding box to the message - // Model collision bounding box is currently broken in Gazebo: - // https://bitbucket.org/osrf/gazebo/issue/1325/getboundingbox-returns-the-models-last - // My suggested fixes are present in the gazebo6-revolve branch - auto bbox = model->GetBoundingBox(); - auto box = response.mutable_boundingbox(); - gz::msgs::Set(box->mutable_min(), bbox.min.Ign()); - gz::msgs::Set(box->mutable_max(), bbox.max.Ign()); - - // Publish the message, serializing the response message in the wrapper data - response.SerializeToString(wrapper.mutable_serialized_data()); - wrapper.set_response("success"); - responsePub_->Publish(wrapper); - std::cout << "Response for request " << currentRequest_ << " sent." << std::endl; - - // Remove all models from the world and advance - world_->Clear(); - this->Advance(); -} - -void BodyAnalyzer::AnalyzeRequest(ConstRequestPtr &request) { - if (request->request() != "analyze_body") { - // Request is not meant for us - return; - } - - if (not request->has_data()) { - std::cerr << "An `analyze_body` request should have data." << std::endl; - return; - } - - // Create the request pair before locking the mutex - this seems - // to prevent threading and "unknown message type" problems. - std::pair req(request->id(), request->data()); - std::cout << "Received request " << request->id() << std::endl; - - boost::mutex::scoped_lock lock(queueMutex_); - - if (requests_.size() >= MAX_QUEUE_SIZE) { - std::cerr << "Ignoring request " << request->id() << ": maximum queue size (" - << MAX_QUEUE_SIZE <<") reached." << std::endl; - return; - } - - // This actually copies the message, but since everything is const - // we don't really have a choice in that regard. This shouldn't - // be the performance bottleneck anyway. - requests_.push(req); - - // Release the lock explicitly to prevent deadlock in ProcessQueue - lock.unlock(); - - this->ProcessQueue(); -} - -void BodyAnalyzer::ProcessQueue() { - // Acquire mutex on queue - boost::mutex::scoped_lock lock(queueMutex_); - - if (requests_.empty()) { - // No requests to handle - return; - } - - boost::mutex::scoped_lock plock(processingMutex_); - if (processing_) { - // Another item is being processed, wait for it - return; - } - - if (world_->GetModelCount()) { - // There are still some items in the world, wait until - // `Clear()` has done its job. - return; - } - - processing_ = true; - - // Pop one request off the queue and set the - // currently processed ID on the class - auto request = requests_.front(); - requests_.pop(); - - currentRequest_ = request.first; - std::cout << "Now handling request: " << currentRequest_ << std::endl; - - // Create model SDF - sdf::SDF robotSDF; - robotSDF.SetFromString(request.second); - - // Force the model name to something we know - std::string name = "analyze_bot_"+boost::lexical_cast(counter_); - robotSDF.Root()->GetElement("model")->GetAttribute("name")->SetFromString(name); - - // Insert the model into the world. For clarity we use `InsertModelString` directly, - // this is actually also what `InsertModelSdf` does. - world_->InsertModelString(robotSDF.ToString()); - - // The contents of the SDF element are *not* cleared automatically - // when robotSDF goes out of scope. - // https://bitbucket.org/osrf/sdformat/issues/104/memory-leak-in-element - robotSDF.Root()->Reset(); - - // Analysis will proceed once the model insertion message comes through -} - -} // namespace gazebo -} // namespace revolve +namespace revolve +{ + namespace gazebo + { + void BodyAnalyzer::Load(gz::physics::WorldPtr world, + sdf::ElementPtr /*_sdf*/) + { + // Store pointer to the world + world_ = world; + + // Pause the world if it is not already paused + world->SetPaused(true); + + // Set initial state values + processing_ = false; + counter_ = 0; + currentRequest_ = -1; + + // Create a new transport node for advertising / subscribing + node_.reset(new gz::transport::Node()); + node_->Init(); + + // Subscribe to the contacts message. Note that the contact manager does + // not generate data without at least one subscriber, so there is no use + // in bypassing the messaging mechanism. + contactsSub_ = node_->Subscribe("~/physics/contacts", + &BodyAnalyzer::OnContacts, + this); + + // Subscribe to analysis request messages + requestSub_ = node_->Subscribe("~/request", + &BodyAnalyzer::AnalyzeRequest, + this); + + // Since models are added asynchronously, we need some way of detecting + // our model add. + // We do this using a model info subscriber. + modelSub_ = + node_->Subscribe("~/model/info", &BodyAnalyzer::OnModel, this); + + // Publisher for the results + responsePub_ = node_->Advertise< gz::msgs::Response >("~/response"); + + // Subscribe to model delete events + deleteSub_ = node_->Subscribe("~/response", + &BodyAnalyzer::OnModelDelete, + this); + + // Hello world! + std::cout << "Body analyzer ready" << std::endl; + } + + void BodyAnalyzer::OnModel(ConstModelPtr &msg) + { + std::string expectedName = + "analyze_bot_" + boost::lexical_cast< std::string >(counter_); + if (msg->name() != expectedName) + { + throw std::runtime_error("INTERNAL ERROR: Expecting model with name '" + + expectedName + "', created model was '" + + msg->name() + "'."); + } + + if (world_->GetModels().size() > 1) + { + throw std::runtime_error("INTERNAL ERROR: Too many models in analyzer"); + } + + // Unpause the world so contacts will be handled + world_->SetPaused(false); + } + + void BodyAnalyzer::OnModelDelete(ConstResponsePtr &msg) + { + if (msg->request() == "entity_delete") + { + // This might have cleared the world + this->ProcessQueue(); + } + } + + void BodyAnalyzer::Advance() + { + processingMutex_.lock(); + + // Clear current request to indicate nothing is being + // processed, and move on to the next item if there is one. + counter_++; + processing_ = false; + currentRequest_ = -1; + + // ProcessQueue needs the processing mutex, release it + processingMutex_.unlock(); + this->ProcessQueue(); + } + + void BodyAnalyzer::OnContacts(ConstContactsPtr &msg) + { + // Pause the world so no new contacts will come in + world_->SetPaused(true); + + boost::mutex::scoped_lock plock(processingMutex_); + if (not processing_) + { + return; + } + plock.unlock(); + + if (world_->GetModelCount() > 1) + { + std::cerr << "WARNING: Too many models in the world." << std::endl; + } + + // Create a response + msgs::BodyAnalysisResponse response; + gz::msgs::Response wrapper; + wrapper.set_id(currentRequest_); + wrapper.set_request("analyze_body"); + + // Add contact info + for (auto contact : msg->contact()) + { + auto msgContact = response.add_contact(); + msgContact->set_collision1(contact.collision1()); + msgContact->set_collision2(contact.collision2()); + } + + std::string name = + "analyze_bot_" + boost::lexical_cast< std::string >(counter_); + gz::physics::ModelPtr model = world_->GetModel(name); + + if (not model) + { + std::cerr << "------------------------------------" << std::endl; + std::cerr + << "INTERNAL ERROR, contact model not found: " + << name + << std::endl; + std::cerr << "Please retry this request." << std::endl; + std::cerr << "------------------------------------" << std::endl; + wrapper.set_response("error"); + response.SerializeToString(wrapper.mutable_serialized_data()); + responsePub_->Publish(wrapper); + + // Advance manually + this->Advance(); + return; + } + + // Add the bounding box to the message + // Model collision bounding box is currently broken in Gazebo: + // https://bitbucket.org/osrf/gazebo/issue/1325/ + // getboundingbox-returns-the-models-last + // My suggested fixes are present in the gazebo6-revolve branch + auto bbox = model->GetBoundingBox(); + auto box = response.mutable_boundingbox(); + gz::msgs::Set(box->mutable_min(), bbox.min.Ign()); + gz::msgs::Set(box->mutable_max(), bbox.max.Ign()); + + // Publish the message, serializing the response message in + // the wrapper data + response.SerializeToString(wrapper.mutable_serialized_data()); + wrapper.set_response("success"); + responsePub_->Publish(wrapper); + std::cout + << "Response for request " + << currentRequest_ + << " sent." + << std::endl; + + // Remove all models from the world and advance + world_->Clear(); + this->Advance(); + } + + void BodyAnalyzer::AnalyzeRequest(ConstRequestPtr &request) + { + if (request->request() != "analyze_body") + { + // Request is not meant for us + return; + } + + if (not request->has_data()) + { + std::cerr << "An `analyze_body` request should have data." << std::endl; + return; + } + + // Create the request pair before locking the mutex - this seems + // to prevent threading and "unknown message type" problems. + std::pair< int, std::string > req(request->id(), request->data()); + std::cout << "Received request " << request->id() << std::endl; + + boost::mutex::scoped_lock lock(queueMutex_); + + if (requests_.size() >= MAX_QUEUE_SIZE) + { + std::cerr + << "Ignoring request " + << request->id() + << ": maximum queue size (" + << MAX_QUEUE_SIZE + << ") reached." + << std::endl; + return; + } + + // This actually copies the message, but since everything is const + // we don't really have a choice in that regard. This shouldn't + // be the performance bottleneck anyway. + requests_.push(req); + + // Release the lock explicitly to prevent deadlock in ProcessQueue + lock.unlock(); + + this->ProcessQueue(); + } + + void BodyAnalyzer::ProcessQueue() + { + // Acquire mutex on queue + boost::mutex::scoped_lock lock(queueMutex_); + + if (requests_.empty()) + { + // No requests to handle + return; + } + + boost::mutex::scoped_lock plock(processingMutex_); + if (processing_) + { + // Another item is being processed, wait for it + return; + } + + if (world_->GetModelCount()) + { + // There are still some items in the world, wait until + // `Clear()` has done its job. + return; + } + + processing_ = true; + + // Pop one request off the queue and set the + // currently processed ID on the class + auto request = requests_.front(); + requests_.pop(); + + currentRequest_ = request.first; + std::cout << "Now handling request: " << currentRequest_ << std::endl; + + // Create model SDF + sdf::SDF robotSDF; + robotSDF.SetFromString(request.second); + + // Force the model name to something we know + std::string name = + "analyze_bot_" + boost::lexical_cast< std::string >(counter_); + robotSDF.Root()->GetElement("model")->GetAttribute("name")->SetFromString( + name); + + // Insert the model into the world. For clarity we use + // `InsertModelString` directly, + // this is actually also what `InsertModelSdf` does. + world_->InsertModelString(robotSDF.ToString()); + + // The contents of the SDF element are *not* cleared automatically + // when robotSDF goes out of scope. + // https://bitbucket.org/osrf/sdformat/issues/104/memory-leak-in-element + robotSDF.Root()->Reset(); + + // Analysis will proceed once the model insertion message comes through + } + } // namespace gazebo +} // namespace revolve diff --git a/cpp/revolve/gazebo/plugin/RobotController.cpp b/cpp/revolve/gazebo/plugin/RobotController.cpp index 446c45c677..3592f92ad5 100644 --- a/cpp/revolve/gazebo/plugin/RobotController.cpp +++ b/cpp/revolve/gazebo/plugin/RobotController.cpp @@ -24,201 +24,234 @@ #include #include -#include #include "RobotController.h" -#include -#include - namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -RobotController::RobotController(): - // Default actuation time, this will be overwritten - // by the plugin config in Load. - actuationTime_(0) -{} - -RobotController::~RobotController() +namespace revolve { - this->node_.reset(); - this->world.reset(); - this->motorFactory_.reset(); - this->sensorFactory_.reset(); -} - -void RobotController::Load(::gazebo::physics::ModelPtr _parent, - sdf::ElementPtr _sdf) { - // Store the pointer to the model / world - this->model = _parent; - this->world = _parent->GetWorld(); - this->initTime_ = this->world->GetSimTime().Double(); - - // Create transport node - node_.reset(new gz::transport::Node()); - node_->Init(); - - // Subscribe to robot battery state updater - batterySetSub_ = node_->Subscribe("~/battery_level/request", &RobotController::UpdateBattery, this); - batterySetPub_ = node_->Advertise("~/battery_level/response"); - - if (not _sdf->HasElement("rv:robot_config")) { - std::cerr << "No `rv:robot_config` element found, controller not initialized." - << std::endl; - return; - } - - auto settings = _sdf->GetElement("rv:robot_config"); - - if (settings->HasElement("rv:update_rate")) { - double updateRate = settings->GetElement("rv:update_rate")->Get< double >(); - actuationTime_ = 1.0 / updateRate; - } - - // Load motors - this->motorFactory_ = this->getMotorFactory(_parent); - this->LoadMotors(settings); - - // Load sensors - this->sensorFactory_ = this->getSensorFactory(_parent); - this->LoadSensors(settings); - - // Load brain, this needs to be done after the motors and - // sensors so they can potentially be reordered. - this->LoadBrain(settings); - - // Call the battery loader - this->LoadBattery(settings); - - // Call startup function which decides on actuation - this->startup(_parent, _sdf); -} - -//////////////////////////////////////////////////////////////// - -void RobotController::UpdateBattery(ConstRequestPtr &request) { - if (request->data() != this->model->GetName() && request->data() != this->model->GetScopedName()) { - return; - } - - gz::msgs::Response resp; - resp.set_id(request->id()); - resp.set_request(request->request()); - - if (request->request() == "set_battery_level") { - resp.set_response("success"); - this->SetBatteryLevel(request->dbl_data()); - } else { - std::stringstream ss; - ss << this->GetBatteryLevel(); - resp.set_response(ss.str()); - } - - batterySetPub_->Publish(resp); -} - -//////////////////////////////////////////////////////////////// - -void RobotController::LoadMotors(sdf::ElementPtr sdf) { - if (not sdf->HasElement("rv:motor")) { - return; - } - - auto motor = sdf->GetElement("rv:motor"); - while (motor) { - auto motorObj = this->motorFactory_->create(motor); - motors_.push_back(motorObj); - motor = motor->GetNextElement("rv:motor"); + namespace gazebo + { + RobotController::RobotController() + // Default actuation time, this will be overwritten + // by the plugin config in Load. + : actuationTime_(0) + {} + + RobotController::~RobotController() + { + this->node_.reset(); + this->world.reset(); + this->motorFactory_.reset(); + this->sensorFactory_.reset(); } -} -//////////////////////////////////////////////////////////// - -void RobotController::LoadSensors(sdf::ElementPtr sdf) { - if (not sdf->HasElement("rv:sensor")) { - return; - } + void RobotController::Load(::gazebo::physics::ModelPtr _parent, + sdf::ElementPtr _sdf) + { + // Store the pointer to the model / world + this->model = _parent; + this->world = _parent->GetWorld(); + this->initTime_ = this->world->GetSimTime().Double(); + + // Create transport node + node_.reset(new gz::transport::Node()); + node_->Init(); + + // Subscribe to robot battery state updater + batterySetSub_ = node_->Subscribe("~/battery_level/request", + &RobotController::UpdateBattery, + this); + batterySetPub_ = + node_->Advertise< gz::msgs::Response >( + "~/battery_level/response"); + + if (not _sdf->HasElement("rv:robot_config")) + { + std::cerr << "No `rv:robot_config` element found, " + "controller not initialized." << std::endl; + return; + } + + auto settings = _sdf->GetElement("rv:robot_config"); + + if (settings->HasElement("rv:update_rate")) + { + double updateRate = + settings->GetElement("rv:update_rate")->Get< double >(); + actuationTime_ = 1.0 / updateRate; + } + + // Load motors + this->motorFactory_ = this->getMotorFactory(_parent); + this->LoadMotors(settings); + + // Load sensors + this->sensorFactory_ = this->getSensorFactory(_parent); + this->LoadSensors(settings); + + // Load brain, this needs to be done after the motors and + // sensors so they can potentially be reordered. + this->LoadBrain(settings); + + // Call the battery loader + this->LoadBattery(settings); + + // Call startup function which decides on actuation + this->startup(_parent, _sdf); + } - auto sensor = sdf->GetElement("rv:sensor"); - while (sensor) { - auto sensorObj = this->sensorFactory_->create(sensor); - sensors_.push_back(sensorObj); - sensor = sensor->GetNextElement("rv:sensor"); - } -} +///////////////////////////////////////////////// + void RobotController::UpdateBattery(ConstRequestPtr &request) + { + if (request->data() != this->model->GetName() + && request->data() != this->model->GetScopedName()) + { + return; + } + + gz::msgs::Response resp; + resp.set_id(request->id()); + resp.set_request(request->request()); + + if (request->request() == "set_battery_level") + { + resp.set_response("success"); + this->SetBatteryLevel(request->dbl_data()); + } + else + { + std::stringstream ss; + ss << this->GetBatteryLevel(); + resp.set_response(ss.str()); + } + + batterySetPub_->Publish(resp); + } -MotorFactoryPtr RobotController::getMotorFactory( - ::gazebo::physics::ModelPtr model) { - return MotorFactoryPtr(new MotorFactory(model)); -} +///////////////////////////////////////////////// + void RobotController::LoadMotors(sdf::ElementPtr sdf) + { + if (not sdf->HasElement("rv:motor")) + { + return; + } + + auto motor = sdf->GetElement("rv:motor"); + while (motor) + { + auto motorObj = this->motorFactory_->create(motor); + motors_.push_back(motorObj); + motor = motor->GetNextElement("rv:motor"); + } + } -SensorFactoryPtr RobotController::getSensorFactory( - ::gazebo::physics::ModelPtr model) { - return SensorFactoryPtr(new SensorFactory(model)); -} +///////////////////////////////////////////////// + void RobotController::LoadSensors(sdf::ElementPtr sdf) + { + if (not sdf->HasElement("rv:sensor")) + { + return; + } + + auto sensor = sdf->GetElement("rv:sensor"); + while (sensor) + { + auto sensorObj = this->sensorFactory_->create(sensor); + sensors_.push_back(sensorObj); + sensor = sensor->GetNextElement("rv:sensor"); + } + } -void RobotController::LoadBrain(sdf::ElementPtr sdf) { - if (not sdf->HasElement("rv:brain")) { - std::cerr << "No robot brain detected, this is probably an error." << std::endl; - return; - } - auto brain = sdf->GetElement("rv:brain"); - brain_.reset(new NeuralNetwork(this->model->GetName(), brain, motors_, sensors_)); -} + MotorFactoryPtr RobotController::getMotorFactory( + ::gazebo::physics::ModelPtr model) + { + return MotorFactoryPtr(new MotorFactory(model)); + } + SensorFactoryPtr RobotController::getSensorFactory( + ::gazebo::physics::ModelPtr model) + { + return SensorFactoryPtr(new SensorFactory(model)); + } -//////////////////////////////////////////////////////////////////////////////// + void RobotController::LoadBrain(sdf::ElementPtr sdf) + { + if (not sdf->HasElement("rv:brain")) + { + std::cerr + << "No robot brain detected, this is probably an error." + << std::endl; + return; + } + auto brain = sdf->GetElement("rv:brain"); + brain_.reset(new NeuralNetwork(this->model->GetName(), + brain, + motors_, + sensors_)); + } +///////////////////////////////////////////////// // Default startup, bind to CheckUpdate -void RobotController::startup(::gazebo::physics::ModelPtr /*_parent*/, sdf::ElementPtr /*_sdf*/) { - this->updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin( - boost::bind(&RobotController::CheckUpdate, this, _1)); -} - -////////////////////////////////////////////////////////////////////////////////// - -void RobotController::CheckUpdate(const ::gazebo::common::UpdateInfo info) { - auto diff = info.simTime - lastActuationTime_; + void RobotController::startup(::gazebo::physics::ModelPtr /*_parent*/, + sdf::ElementPtr /*_sdf*/) + { + this->updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin( + boost::bind(&RobotController::CheckUpdate, this, _1)); + } - if (diff.Double() > actuationTime_) { - this->DoUpdate(info); - lastActuationTime_ = info.simTime; - } -} +///////////////////////////////////////////////// + void RobotController::CheckUpdate(const ::gazebo::common::UpdateInfo info) + { + auto diff = info.simTime - lastActuationTime_; -///////////////////////////////////////////////////////////////////////////////////// + if (diff.Double() > actuationTime_) + { + this->DoUpdate(info); + lastActuationTime_ = info.simTime; + } + } +///////////////////////////////////////////////// // Default update function simply tells the brain to perform an update -void RobotController::DoUpdate(const ::gazebo::common::UpdateInfo info) { - brain_->update(motors_, sensors_, info.simTime.Double() - initTime_, actuationTime_); -} + void RobotController::DoUpdate(const ::gazebo::common::UpdateInfo info) + { + brain_->update(motors_, + sensors_, + info.simTime.Double() - initTime_, + actuationTime_); + } -///////////////////////////////////////////////////////// +///////////////////////////////////////////////// -void RobotController::LoadBattery(sdf::ElementPtr sdf) { - if (sdf->HasElement("rv:battery")) { - this->batteryElem_ = sdf->GetElement("rv:battery"); - } -} + void RobotController::LoadBattery(sdf::ElementPtr sdf) + { + if (sdf->HasElement("rv:battery")) + { + this->batteryElem_ = sdf->GetElement("rv:battery"); + } + } -/////////////////////////////////////////////////////////// -double RobotController::GetBatteryLevel() { - if (not batteryElem_ || not batteryElem_->HasElement("rv:level")) { - return 0.0; - } +///////////////////////////////////////////////// + double RobotController::GetBatteryLevel() + { + if (not batteryElem_ || not batteryElem_->HasElement("rv:level")) + { + return 0.0; + } - return batteryElem_->GetElement("rv:level")->Get< double >(); -} + return batteryElem_->GetElement("rv:level")->Get< double >(); + } -///////////////////////////////////////////////////////////////// -void RobotController::SetBatteryLevel(double level) { - if (batteryElem_ && batteryElem_->HasElement("rv:level")) { - batteryElem_->GetElement("rv:level")->Set(level); - } -} +///////////////////////////////////////////////// + void RobotController::SetBatteryLevel(double level) + { + if (batteryElem_ && batteryElem_->HasElement("rv:level")) + { + batteryElem_->GetElement("rv:level")->Set(level); + } + } -} /* namespace gazebo */ + } /* namespace gazebo */ } /* namespace revolve */ diff --git a/cpp/revolve/gazebo/plugin/WorldController.cpp b/cpp/revolve/gazebo/plugin/WorldController.cpp index 71b17fa7ac..4c052b8c10 100644 --- a/cpp/revolve/gazebo/plugin/WorldController.cpp +++ b/cpp/revolve/gazebo/plugin/WorldController.cpp @@ -19,205 +19,248 @@ #include "WorldController.h" -#include - namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -WorldController::WorldController(): - robotStatesPubFreq_(0), - lastRobotStatesUpdateTime_(0) +namespace revolve { -} - -void WorldController::Load(gz::physics::WorldPtr world, sdf::ElementPtr /*_sdf*/) { - std::cout << "World plugin loaded." << std::endl; - - // Store the world - world_ = world; + namespace gazebo + { + WorldController::WorldController() : + robotStatesPubFreq_(0), lastRobotStatesUpdateTime_(0) + { + } - // Create transport node - node_.reset(new gz::transport::Node()); - node_->Init(); + void WorldController::Load(gz::physics::WorldPtr world, + sdf::ElementPtr /*_sdf*/) + { + std::cout << "World plugin loaded." << std::endl; - // Subscribe to insert request messages - requestSub_ = node_->Subscribe("~/request", &WorldController::HandleRequest, this); + // Store the world + world_ = world; - // Publisher for `entity_delete` requests. - requestPub_ = node_->Advertise("~/request"); + // Create transport node + node_.reset(new gz::transport::Node()); + node_->Init(); - // Publisher for inserted models - responseSub_ = node_->Subscribe("~/response", &WorldController::HandleResponse, this); + // Subscribe to insert request messages + requestSub_ = node_->Subscribe("~/request", + &WorldController::HandleRequest, + this); - // Publisher for inserted models - responsePub_ = node_->Advertise("~/response"); + // Publisher for `entity_delete` requests. + requestPub_ = node_->Advertise< gz::msgs::Request >("~/request"); - // Since models are added asynchronously, we need some way of detecting our model add. - // We do this using a model info subscriber. - modelSub_ = node_->Subscribe("~/model/info", &WorldController::OnModel, this); + // Publisher for inserted models + responseSub_ = node_->Subscribe("~/response", + &WorldController::HandleResponse, + this); - // Bind to the world update event to perform some logic - updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin( - boost::bind(&WorldController::OnUpdate, this, _1)); + // Publisher for inserted models + responsePub_ = node_->Advertise< gz::msgs::Response >("~/response"); - // Robot pose publisher - robotStatesPub_ = node_->Advertise("~/revolve/robot_states", 50); -} + // Since models are added asynchronously, we need some way of detecting + // our model add. + // We do this using a model info subscriber. + modelSub_ = + node_->Subscribe("~/model/info", &WorldController::OnModel, this); -void WorldController::OnUpdate(const ::gazebo::common::UpdateInfo &_info) { - if (not robotStatesPubFreq_) { - return; - } + // Bind to the world update event to perform some logic + updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin( + boost::bind(&WorldController::OnUpdate, this, _1)); - double secs = 1.0 / robotStatesPubFreq_; - double time = _info.simTime.Double(); - if ((time - lastRobotStatesUpdateTime_) >= secs) { - // Send robot info update message, this only sends the - // main pose of the robot (which is all we need for now) - msgs::RobotStates msg; - gz::msgs::Set(msg.mutable_time(), _info.simTime); + // Robot pose publisher + robotStatesPub_ = node_->Advertise< revolve::msgs::RobotStates >( + "~/revolve/robot_states", 50); + } - for (auto model : world_->GetModels()) { - if (model->IsStatic()) { - // Ignore static models such as the ground and obstacles - continue; + void WorldController::OnUpdate(const ::gazebo::common::UpdateInfo &_info) + { + if (not robotStatesPubFreq_) + { + return; } - msgs::RobotState *stateMsg = msg.add_robot_state(); - stateMsg->set_name(model->GetScopedName()); - stateMsg->set_id(model->GetId()); - - gz::msgs::Pose *poseMsg = stateMsg->mutable_pose(); - gz::msgs::Set(poseMsg, model->GetRelativePose().Ign()); - } - - if (msg.robot_state_size() > 0) { - robotStatesPub_->Publish(msg); - lastRobotStatesUpdateTime_ = time; + double secs = 1.0 / robotStatesPubFreq_; + double time = _info.simTime.Double(); + if ((time - lastRobotStatesUpdateTime_) >= secs) + { + // Send robot info update message, this only sends the + // main pose of the robot (which is all we need for now) + msgs::RobotStates msg; + gz::msgs::Set(msg.mutable_time(), _info.simTime); + + for (auto model : world_->GetModels()) + { + if (model->IsStatic()) + { + // Ignore static models such as the ground and obstacles + continue; + } + + msgs::RobotState *stateMsg = msg.add_robot_state(); + stateMsg->set_name(model->GetScopedName()); + stateMsg->set_id(model->GetId()); + + gz::msgs::Pose *poseMsg = stateMsg->mutable_pose(); + gz::msgs::Set(poseMsg, model->GetRelativePose().Ign()); + } + + if (msg.robot_state_size() > 0) + { + robotStatesPub_->Publish(msg); + lastRobotStatesUpdateTime_ = time; + } + } } - } -} // Process insert and delete requests -void WorldController::HandleRequest(ConstRequestPtr & request) { - if (request->request() == "delete_robot") { - auto name = request->data(); - std::cout << "Processing request `" << request->id() - << "` to delete robot `" << name << "`" << std::endl; - - gz::physics::ModelPtr model = world_->GetModel(name); - if (model) { - // Tell the world to remove the model - // Using `World::RemoveModel()` from here crashes the transport library, the - // cause of which I've yet to figure out - it has something to do - // with race conditions where the model is used by the world while - // it is being updated. Fixing this completely appears to be a rather - // involved process, instead, we'll use an `entity_delete` request, - // which will make sure deleting the model happens on the world thread. - gz::msgs::Request deleteReq; - int id = gz::physics::getUniqueId(); - deleteReq.set_id(id); - deleteReq.set_request("entity_delete"); - deleteReq.set_data(model->GetScopedName()); - - deleteMutex_.lock(); - deleteMap_[id] = request->id(); - deleteMutex_.unlock(); - - requestPub_->Publish(deleteReq); - } else { - std::cerr << "Model `" << name << "` could not be found in the world." << std::endl; - gz::msgs::Response resp; - resp.set_id(request->id()); - resp.set_request("delete_robot"); - resp.set_response("error"); - responsePub_->Publish(resp); + void WorldController::HandleRequest(ConstRequestPtr &request) + { + if (request->request() == "delete_robot") + { + auto name = request->data(); + std::cout << "Processing request `" << request->id() + << "` to delete robot `" << name << "`" << std::endl; + + gz::physics::ModelPtr model = world_->GetModel(name); + if (model) + { + // Tell the world to remove the model + // Using `World::RemoveModel()` from here crashes the transport + // library, the cause of which I've yet to figure out - it has + // something to do with race conditions where the model is used by the + // world while it is being updated. Fixing this completely appears to + // be a rather involved process, instead, we'll use an `entity_delete` + // request, which will make sure deleting the model happens on the + // world thread. + gz::msgs::Request deleteReq; + int id = gz::physics::getUniqueId(); + deleteReq.set_id(id); + deleteReq.set_request("entity_delete"); + deleteReq.set_data(model->GetScopedName()); + + deleteMutex_.lock(); + deleteMap_[id] = request->id(); + deleteMutex_.unlock(); + + requestPub_->Publish(deleteReq); + } + else + { + std::cerr + << "Model `" + << name + << "` could not be found in the world." + << std::endl; + gz::msgs::Response resp; + resp.set_id(request->id()); + resp.set_request("delete_robot"); + resp.set_response("error"); + responsePub_->Publish(resp); + } + } + else if (request->request() == "insert_sdf") + { + std::cout + << "Processing insert model request ID `" + << request->id() + << "`." + << std::endl; + sdf::SDF robotSDF; + robotSDF.SetFromString(request->data()); + + // Get the model name, store in the expected map + auto name = + robotSDF.Root()->GetElement("model") + ->GetAttribute("name")->GetAsString(); + + insertMutex_.lock(); + insertMap_[name] = request->id(); + insertMutex_.unlock(); + + world_->InsertModelString(robotSDF.ToString()); + + // Don't leak memory + // https://bitbucket.org/osrf/sdformat/issues/104/memory-leak-in-element + robotSDF.Root()->Reset(); + } + else if (request->request() == "set_robot_state_update_frequency") + { + robotStatesPubFreq_ = + boost::lexical_cast< unsigned int >(request->data()); + std::cout + << "Setting robot state update frequency to " + << robotStatesPubFreq_ + << "." + << std::endl; + + gz::msgs::Response resp; + resp.set_id(request->id()); + resp.set_request("set_robot_state_update_frequency"); + resp.set_response("success"); + + responsePub_->Publish(resp); + } } - } else if (request->request() == "insert_sdf") { - std::cout << "Processing insert model request ID `" << request->id() << "`." << std::endl; - sdf::SDF robotSDF; - robotSDF.SetFromString(request->data()); - - // Get the model name, store in the expected map - auto name = robotSDF.Root()->GetElement("model")->GetAttribute("name")->GetAsString(); - insertMutex_.lock(); - insertMap_[name] = request->id(); - insertMutex_.unlock(); - - world_->InsertModelString(robotSDF.ToString()); - - // Don't leak memory - // https://bitbucket.org/osrf/sdformat/issues/104/memory-leak-in-element - robotSDF.Root()->Reset(); - } else if (request->request() == "set_robot_state_update_frequency") { - robotStatesPubFreq_ = boost::lexical_cast(request->data()); - std::cout << "Setting robot state update frequency to " << robotStatesPubFreq_ << "." << std::endl; + void WorldController::OnModel(ConstModelPtr &msg) + { + auto name = msg->name(); + + int id; + { + boost::mutex::scoped_lock lock(insertMutex_); + if (insertMap_.count(name) <= 0) + { + // Insert was not requested here, ignore it + return; + } + id = insertMap_[name]; + insertMap_.erase(name); + } - gz::msgs::Response resp; - resp.set_id(request->id()); - resp.set_request("set_robot_state_update_frequency"); - resp.set_response("success"); + // Respond with the inserted model + gz::msgs::Response resp; + resp.set_request("insert_sdf"); + resp.set_response("success"); + resp.set_id(id); - responsePub_->Publish(resp); - } -} + msgs::ModelInserted inserted; + inserted.mutable_model()->CopyFrom(*msg); + gz::msgs::Set(inserted.mutable_time(), world_->GetSimTime()); + inserted.SerializeToString(resp.mutable_serialized_data()); -void WorldController::OnModel(ConstModelPtr &msg) { - auto name = msg->name(); + responsePub_->Publish(resp); - int id; - { - boost::mutex::scoped_lock lock(insertMutex_); - if (insertMap_.count(name) <= 0) { - // Insert was not requested here, ignore it - return; + std::cout << "Model `" << name + << "` inserted, world now contains " << world_->GetModelCount() + << " models." << std::endl; } - id = insertMap_[name]; - insertMap_.erase(name); - } - - // Respond with the inserted model - gz::msgs::Response resp; - resp.set_request("insert_sdf"); - resp.set_response("success"); - resp.set_id(id); - msgs::ModelInserted inserted; - inserted.mutable_model()->CopyFrom(*msg); - gz::msgs::Set(inserted.mutable_time(), world_->GetSimTime()); - inserted.SerializeToString(resp.mutable_serialized_data()); - - responsePub_->Publish(resp); + void WorldController::HandleResponse(ConstResponsePtr &response) + { + if (response->request() != "entity_delete") + { + return; + } - std::cout << "Model `" << name << "` inserted, world now contains " << - world_->GetModelCount() << " models." << std::endl; -} + int id; + { + boost::mutex::scoped_lock lock(deleteMutex_); + if (deleteMap_.count(response->id()) <= 0) + { + return; + } -void WorldController::HandleResponse(ConstResponsePtr &response) { - if (response->request() != "entity_delete") { - return; - } + id = deleteMap_[response->id()]; + deleteMap_.erase(id); + } - int id; - { - boost::mutex::scoped_lock lock(deleteMutex_); - if (deleteMap_.count(response->id()) <= 0) { - return; + gz::msgs::Response resp; + resp.set_id(id); + resp.set_request("delete_robot"); + resp.set_response("success"); + responsePub_->Publish(resp); } - - id = deleteMap_[response->id()]; - deleteMap_.erase(id); - } - - gz::msgs::Response resp; - resp.set_id(id); - resp.set_request("delete_robot"); - resp.set_response("success"); - responsePub_->Publish(resp); -} - -} // namespace gazebo -} // namespace revolve + } // namespace gazebo +} // namespace revolve diff --git a/cpp/revolve/gazebo/sensors/BatterySensor.cpp b/cpp/revolve/gazebo/sensors/BatterySensor.cpp index 0ffc3ea25c..bffe753b8d 100644 --- a/cpp/revolve/gazebo/sensors/BatterySensor.cpp +++ b/cpp/revolve/gazebo/sensors/BatterySensor.cpp @@ -16,41 +16,51 @@ * */ +#include + #include "BatterySensor.h" namespace gz = gazebo; -namespace revolve { -namespace gazebo { +namespace revolve +{ + namespace gazebo + { + BatterySensor::BatterySensor(::gazebo::physics::ModelPtr model, + std::string partId, + std::string sensorId) + : VirtualSensor(model, partId, sensorId, 1) + { + // Find the revolve plugin to get the battery data + auto modelSdf = model->GetSDF(); + if (modelSdf->HasElement("plugin")) + { + auto pluginElem = modelSdf->GetElement("plugin"); + while (pluginElem) + { + if (pluginElem->HasElement("rv:robot_config")) + { + // Found revolve plugin + auto settings = pluginElem->GetElement("rv:robot_config"); + if (settings->HasElement("rv:battery")) + { + this->batteryElem = settings->GetElement("rv:battery"); + } -BatterySensor::BatterySensor(::gazebo::physics::ModelPtr model, std::string partId, - std::string sensorId): - VirtualSensor(model, partId, sensorId, 1) { - // Find the revolve plugin to get the battery data - auto modelSdf = model->GetSDF(); - if (modelSdf->HasElement("plugin")) { - auto pluginElem = modelSdf->GetElement("plugin"); - while (pluginElem) { - if (pluginElem->HasElement("rv:robot_config")) { - // Found revolve plugin - auto settings = pluginElem->GetElement("rv:robot_config"); - if (settings->HasElement("rv:battery")) { - this->batteryElem = settings->GetElement("rv:battery"); + break; + } + pluginElem = pluginElem->GetNextElement("plugin"); } - - break; } - pluginElem = pluginElem->GetNextElement("plugin"); } - } -} /////////////////////////////////// -void BatterySensor::read(double * input) { - input[0] = this->batteryElem && this->batteryElem->HasElement("rv:level") ? - this->batteryElem->GetElement("rv:level")->Get< double >() : 0.0; -} - -} + void BatterySensor::read(double *input) + { + input[0] = + this->batteryElem && this->batteryElem->HasElement("rv:level") ? + this->batteryElem->GetElement("rv:level")->Get< double >() : 0.0; + } + } } diff --git a/cpp/revolve/gazebo/sensors/ImuSensor.cpp b/cpp/revolve/gazebo/sensors/ImuSensor.cpp index 8a0adbc26e..8adc6199b3 100644 --- a/cpp/revolve/gazebo/sensors/ImuSensor.cpp +++ b/cpp/revolve/gazebo/sensors/ImuSensor.cpp @@ -17,52 +17,63 @@ * */ -#include "ImuSensor.h" +#include +#include -#include -#include +#include "ImuSensor.h" namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -ImuSensor::ImuSensor(::gazebo::physics::ModelPtr model, sdf::ElementPtr sensor, - std::string partId, std::string sensorId): - Sensor(model, sensor, partId, sensorId, 6) +namespace revolve { - this->castSensor_ = boost::dynamic_pointer_cast(this->sensor_); - - if (not this->castSensor_) { - std::cerr << "Creating an IMU sensor with a non-IMU sensor object." << std::endl; - throw std::runtime_error("Sensor error"); - } + namespace gazebo + { + ImuSensor::ImuSensor(::gazebo::physics::ModelPtr model, + sdf::ElementPtr sensor, + std::string partId, + std::string sensorId) : + Sensor(model, sensor, partId, sensorId, 6) + { + this->castSensor_ = + boost::dynamic_pointer_cast< gz::sensors::ImuSensor >( + this->sensor_); - // Initialize all initial values to zero - memset(lastValues_, 0, sizeof(lastValues_)); + if (not this->castSensor_) + { + std::cerr << "Creating an IMU sensor with a non-IMU sensor object." + << std::endl; + throw std::runtime_error("Sensor error"); + } - // Add update connection that will produce new value - this->updateConnection_ = this->castSensor_->ConnectUpdated(boost::bind(&ImuSensor::OnUpdate, this)); -} + // Initialize all initial values to zero + std::memset(lastValues_, 0, sizeof(lastValues_)); -ImuSensor::~ImuSensor() {} + // Add update connection that will produce new value + this->updateConnection_ = + this->castSensor_->ConnectUpdated( + boost::bind(&ImuSensor::OnUpdate, this)); + } -void ImuSensor::OnUpdate() { - // Store the recorded values - auto acc = this->castSensor_->LinearAcceleration(); - auto velo = this->castSensor_->AngularVelocity(); - lastValues_[0] = acc[0]; - lastValues_[1] = acc[1]; - lastValues_[2] = acc[2]; - lastValues_[3] = velo[0]; - lastValues_[4] = velo[1]; - lastValues_[5] = velo[2]; -} + ImuSensor::~ImuSensor() + {} -void ImuSensor::read(double * input) { - // Copy our values to the input array - memcpy(input, lastValues_, sizeof(lastValues_)); -} + void ImuSensor::OnUpdate() + { + // Store the recorded values + auto acc = this->castSensor_->LinearAcceleration(); + auto velo = this->castSensor_->AngularVelocity(); + lastValues_[0] = acc[0]; + lastValues_[1] = acc[1]; + lastValues_[2] = acc[2]; + lastValues_[3] = velo[0]; + lastValues_[4] = velo[1]; + lastValues_[5] = velo[2]; + } -} /* namespace gazebo */ -} /* namespace revolve */ + void ImuSensor::read(double *input) + { + // Copy our values to the input array + std::memcpy(input, lastValues_, sizeof(lastValues_)); + } + } /* namespace gazebo */ +} /* namespace revolve */ diff --git a/cpp/revolve/gazebo/sensors/LightSensor.cpp b/cpp/revolve/gazebo/sensors/LightSensor.cpp index 47d7b4bad6..8cf1b9e440 100644 --- a/cpp/revolve/gazebo/sensors/LightSensor.cpp +++ b/cpp/revolve/gazebo/sensors/LightSensor.cpp @@ -17,55 +17,68 @@ * */ -#include +#include #include -namespace gz = gazebo; +#include -namespace revolve { -namespace gazebo { +namespace gz = gazebo; -LightSensor::LightSensor(::gazebo::physics::ModelPtr model, - sdf::ElementPtr sensor, - std::string partId, - std::string sensorId) - : Sensor(model, sensor, partId, sensorId, 1) - , lastValue_(1.0) // Initialize light sensor to full intensity +namespace revolve { - this->castSensor_ = boost::dynamic_pointer_cast(this->sensor_); + namespace gazebo + { + LightSensor::LightSensor(::gazebo::physics::ModelPtr model, + sdf::ElementPtr sensor, + std::string partId, + std::string sensorId) + : Sensor(model, sensor, partId, sensorId, 1) + , lastValue_(1.0) // Initialize light sensor to full intensity + { + this->castSensor_ = + boost::dynamic_pointer_cast< gz::sensors::CameraSensor >( + this->sensor_); - if (not this->castSensor_) { - std::cerr << "Creating a light sensor with a non-camera sensor object." << std::endl; - throw std::runtime_error("Sensor error"); - } + if (not this->castSensor_) + { + std::cerr << "Creating a light sensor with a non-camera sensor object." + << std::endl; + throw std::runtime_error("Sensor error"); + } - // Sensor must always update - this->castSensor_->SetActive(true); + // Sensor must always update + this->castSensor_->SetActive(true); - // One byte per channel per pixel - this->dataSize_ = 3 * this->castSensor_->GetImageWidth() * this->castSensor_->GetImageHeight(); + // One byte per channel per pixel + this->dataSize_ = + 3 * this->castSensor_->GetImageWidth() + * this->castSensor_->GetImageHeight(); - // Add update connection that will produce new value - this->updateConnection_ = this->sensor_->ConnectUpdated(boost::bind(&LightSensor::OnUpdate, this)); -} + // Add update connection that will produce new value + this->updateConnection_ = + this->sensor_->ConnectUpdated(boost::bind(&LightSensor::OnUpdate, + this)); + } -LightSensor::~LightSensor() -{} + LightSensor::~LightSensor() + {} -void LightSensor::OnUpdate() { - // Average all channels and pixels to get a linear - // light intensity. - const unsigned char* data = this->castSensor_->GetImageData(); - float avg = 0; - for (unsigned int i = 0; i < this->dataSize_; ++i) { - avg += (unsigned int)data[i]; - } + void LightSensor::OnUpdate() + { + // Average all channels and pixels to get a linear + // light intensity. + const unsigned char *data = this->castSensor_->GetImageData(); + float avg = 0; + for (unsigned int i = 0; i < this->dataSize_; ++i) + { + avg += (unsigned int)data[i]; + } - avg /= this->dataSize_ * 255.f; + avg /= this->dataSize_ * 255.f; - this->lastValue_ = avg; -} + this->lastValue_ = avg; + } /** * TODO Measure the fastest / most accurate way to use the light sensor. @@ -81,9 +94,9 @@ void LightSensor::OnUpdate() { * case that would force the sensor update here on the "driver" * thread, which might be detrimental to performance. */ -void LightSensor::read(double * input) { - input[0] = lastValue_; -} - -} /* namespace gazebo */ -} /* namespace tol_robogen */ + void LightSensor::read(double *input) + { + input[0] = lastValue_; + } + } /* namespace gazebo */ +} /* namespace tol_robogen */ diff --git a/cpp/revolve/gazebo/sensors/PointIntensitySensor.cpp b/cpp/revolve/gazebo/sensors/PointIntensitySensor.cpp index 918c7626e5..af93cd9c16 100644 --- a/cpp/revolve/gazebo/sensors/PointIntensitySensor.cpp +++ b/cpp/revolve/gazebo/sensors/PointIntensitySensor.cpp @@ -16,59 +16,73 @@ * */ +#include + #include "PointIntensitySensor.h" namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -PointIntensitySensor::PointIntensitySensor(sdf::ElementPtr sensor, - ::gazebo::physics::ModelPtr model, - std::string partId, std::string sensorId) - : VirtualSensor(model, partId, sensorId, 1) - , i_max_(1) - , r_(1) +namespace revolve { + namespace gazebo + { + PointIntensitySensor::PointIntensitySensor( + sdf::ElementPtr sensor, + ::gazebo::physics::ModelPtr model, + std::string partId, + std::string sensorId) + : VirtualSensor(model, partId, sensorId, 1) + , i_max_(1) + , r_(1) + { + if (not sensor->HasElement("rv:point_intensity_sensor")) + { + std::cerr << "PointIntensitySensor missing " + "`rv:point_intensity_sensor` element." << std::endl; + throw std::runtime_error("Robot brain error."); + } - if (not sensor->HasElement("rv:point_intensity_sensor")) { - std::cerr << "PointIntensitySensor missing `rv:point_intensity_sensor` element." << std::endl; - throw std::runtime_error("Robot brain error."); - } - - auto configElem = sensor->GetElement("rv:point_intensity_sensor"); + auto configElem = sensor->GetElement("rv:point_intensity_sensor"); - if (not configElem->HasElement("rv:point")) { - std::cerr << "PointIntensitySensor missing `rv:point` element." << std::endl; - } + if (not configElem->HasElement("rv:point")) + { + std::cerr << "PointIntensitySensor missing " + "`rv:point` element." << std::endl; + } - auto pointElem = configElem->GetElement("rv:point"); - this->point_ = pointElem->Get< gz::math::Vector3 >(); + auto pointElem = configElem->GetElement("rv:point"); + this->point_ = pointElem->Get< gz::math::Vector3 >(); - if (configElem->HasElement("rv:function")) { - auto funcElem = configElem->GetElement("rv:function"); + if (configElem->HasElement("rv:function")) + { + auto funcElem = configElem->GetElement("rv:function"); - if (funcElem->HasAttribute("r")) { - funcElem->GetAttribute("r")->Get(this->r_); - } + if (funcElem->HasAttribute("r")) + { + funcElem->GetAttribute("r")->Get(this->r_); + } - if (funcElem->HasAttribute("i_max")) { - funcElem->GetAttribute("i_max")->Get(this->i_max_); + if (funcElem->HasAttribute("i_max")) + { + funcElem->GetAttribute("i_max")->Get(this->i_max_); + } + } } - } -} /////////////////////////////////// -void PointIntensitySensor::read(double * input) { - double distance = this->model_->GetWorldPose().pos.Distance(this->point_); + void PointIntensitySensor::read(double *input) + { + double distance = this->model_->GetWorldPose().pos.Distance(this->point_); - if (distance < this->r_) { - input[0] = this->i_max_; - } else { - input[0] = this->i_max_ * pow(this->r_ / distance, 2); + if (distance < this->r_) + { + input[0] = this->i_max_; + } + else + { + input[0] = this->i_max_ * pow(this->r_ / distance, 2); + } + } } } - -} -} diff --git a/cpp/revolve/gazebo/sensors/Sensor.cpp b/cpp/revolve/gazebo/sensors/Sensor.cpp index 13262e5180..f984271163 100644 --- a/cpp/revolve/gazebo/sensors/Sensor.cpp +++ b/cpp/revolve/gazebo/sensors/Sensor.cpp @@ -17,51 +17,60 @@ * */ +#include + #include namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -Sensor::Sensor(::gazebo::physics::ModelPtr model, - sdf::ElementPtr sensor, - std::string partId, - std::string sensorId, - unsigned int inputs) - : VirtualSensor(model, partId, sensorId, inputs) +namespace revolve { - if (not sensor->HasAttribute("sensor") || not sensor->HasAttribute("link")) { - std::cerr << "Sensor is missing required attributes (`link` or `sensor`)." << std::endl; - throw std::runtime_error("Sensor error"); - } - - auto sensorName = sensor->GetAttribute("sensor")->GetAsString(); - auto linkName = sensor->GetAttribute("link")->GetAsString(); + namespace gazebo + { + Sensor::Sensor(::gazebo::physics::ModelPtr model, + sdf::ElementPtr sensor, + std::string partId, + std::string sensorId, + unsigned int inputs) + : VirtualSensor(model, partId, sensorId, inputs) + { + if (not sensor->HasAttribute("sensor") + || not sensor->HasAttribute("link")) + { + std::cerr + << "Sensor is missing required attributes (`link` or `sensor`)." + << std::endl; + throw std::runtime_error("Sensor error"); + } - auto link = model->GetLink(linkName); - if (not link) { - std::cerr << "Link '" << linkName << "' for sensor '" - << sensorName << "' is not present in model." << std::endl; - throw std::runtime_error("Sensor error"); - } + auto sensorName = sensor->GetAttribute("sensor")->GetAsString(); + auto linkName = sensor->GetAttribute("link")->GetAsString(); - std::string scopedName = link->GetScopedName(true) + "::" + sensorName; - this->sensor_ = gz::sensors::get_sensor(scopedName); + auto link = model->GetLink(linkName); + if (not link) + { + std::cerr << "Link '" << linkName << "' for sensor '" + << sensorName << "' is not present in model." << std::endl; + throw std::runtime_error("Sensor error"); + } - if (not this->sensor_) { - std::cerr << "Sensor with scoped name '" << scopedName - << "' could not be found." << std::endl; - throw std::runtime_error("Sensor error"); - } -} + std::string scopedName = link->GetScopedName(true) + "::" + sensorName; + this->sensor_ = gz::sensors::get_sensor(scopedName); -Sensor::~Sensor() -{} + if (not this->sensor_) + { + std::cerr << "Sensor with scoped name '" << scopedName + << "' could not be found." << std::endl; + throw std::runtime_error("Sensor error"); + } + } -::gazebo::sensors::SensorPtr Sensor::gzSensor() { - return sensor_; -} + Sensor::~Sensor() + {} -} /* namespace gazebo */ -} /* namespace tol_robogen */ + ::gazebo::sensors::SensorPtr Sensor::gzSensor() + { + return sensor_; + } + } /* namespace gazebo */ +} /* namespace tol_robogen */ diff --git a/cpp/revolve/gazebo/sensors/SensorFactory.cpp b/cpp/revolve/gazebo/sensors/SensorFactory.cpp index 241cfb3c4f..34b38a6672 100644 --- a/cpp/revolve/gazebo/sensors/SensorFactory.cpp +++ b/cpp/revolve/gazebo/sensors/SensorFactory.cpp @@ -17,66 +17,88 @@ * */ +#include +#include +#include + #include #include -#include -#include - namespace gz = gazebo; -namespace revolve { -namespace gazebo { - -SensorFactory::SensorFactory(gz::physics::ModelPtr model): - model_(model) -{} - -SensorFactory::~SensorFactory() -{} +namespace revolve +{ + namespace gazebo + { + SensorFactory::SensorFactory(gz::physics::ModelPtr model) + : model_(model) + {} -SensorPtr SensorFactory::getSensor(sdf::ElementPtr sensor, - const std::string& type, - const std::string& partId, - const std::string& sensorId) { - SensorPtr out; - if ("imu" == type) { - out.reset(new ImuSensor(this->model_, sensor, partId, sensorId)); - } else if ("light" == type) { - out.reset(new LightSensor(this->model_, sensor, partId, sensorId)); - } else if ("touch" == type) { - out.reset(new TouchSensor(this->model_, sensor, partId, sensorId)); - } else if ("basic_battery" == type) { - out.reset(new BatterySensor(this->model_, partId, sensorId)); - } else if ("point_intensity" == type) { - out.reset(new PointIntensitySensor(sensor, this->model_, partId, sensorId)); - } + SensorFactory::~SensorFactory() + {} - return out; -} + SensorPtr SensorFactory::getSensor(sdf::ElementPtr sensor, + const std::string &type, + const std::string &partId, + const std::string &sensorId) + { + SensorPtr out; + if ("imu" == type) + { + out.reset(new ImuSensor(this->model_, sensor, partId, sensorId)); + } + else if ("light" == type) + { + out.reset(new LightSensor(this->model_, sensor, partId, sensorId)); + } + else if ("touch" == type) + { + out.reset(new TouchSensor(this->model_, sensor, partId, sensorId)); + } + else if ("basic_battery" == type) + { + out.reset(new BatterySensor(this->model_, partId, sensorId)); + } + else if ("point_intensity" == type) + { + out.reset(new PointIntensitySensor(sensor, + this->model_, + partId, + sensorId)); + } -SensorPtr SensorFactory::create(sdf::ElementPtr sensor) { - auto typeParam = sensor->GetAttribute("type"); - auto partIdParam = sensor->GetAttribute("part_id"); - auto idParam = sensor->GetAttribute("id"); + return out; + } - if (not typeParam || not partIdParam || not idParam) { - std::cerr << "Sensor is missing required attributes (`id`, `type` or `part_id`)." << std::endl; - throw std::runtime_error("Sensor error"); - } + SensorPtr SensorFactory::create(sdf::ElementPtr sensor) + { + auto typeParam = sensor->GetAttribute("type"); + auto partIdParam = sensor->GetAttribute("part_id"); + auto idParam = sensor->GetAttribute("id"); - auto partId = partIdParam->GetAsString(); - auto type = typeParam->GetAsString(); - auto id = idParam->GetAsString(); + if (not typeParam || not partIdParam || not idParam) + { + std::cerr << "Sensor is missing required attributes " + "(`id`, `type` or `part_id`)." << std::endl; + throw std::runtime_error("Sensor error"); + } - SensorPtr out = this->getSensor(sensor, type, partId, id); - if (not out) { - std::cerr << "Sensor type '" << type << "' is not supported." << std::endl; - throw std::runtime_error("Sensor error"); - } + auto partId = partIdParam->GetAsString(); + auto type = typeParam->GetAsString(); + auto id = idParam->GetAsString(); - return out; -} + SensorPtr out = this->getSensor(sensor, type, partId, id); + if (not out) + { + std::cerr + << "Sensor type '" + << type + << "' is not supported." + << std::endl; + throw std::runtime_error("Sensor error"); + } -} /* namespace gazebo */ -} /* namespace tol_robogen */ + return out; + } + } /* namespace gazebo */ +} /* namespace tol_robogen */ diff --git a/cpp/revolve/gazebo/sensors/TouchSensor.cpp b/cpp/revolve/gazebo/sensors/TouchSensor.cpp index 98d674b7f2..fd5b042b81 100644 --- a/cpp/revolve/gazebo/sensors/TouchSensor.cpp +++ b/cpp/revolve/gazebo/sensors/TouchSensor.cpp @@ -16,55 +16,76 @@ * Author: Elte Hupkes * */ - /* - * TouchSensor.cpp - * - * Created on: Mar 27, 2015 - * Author: elte - */ +* Copyright (C) 2017 Vrije Universiteit Amsterdam +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +* +* Author: Elte Hupkes +* Date: March 27, 2015 +* +*/ -#include +#include #include -namespace gz = gazebo; +#include -namespace revolve { -namespace gazebo { +namespace gz = gazebo; -TouchSensor::TouchSensor(::gazebo::physics::ModelPtr model, - sdf::ElementPtr sensor, - std::string partId, - std::string sensorId) - : Sensor(model, sensor, partId, sensorId, 1) - , lastValue_(false) +namespace revolve { - this->castSensor_ = boost::dynamic_pointer_cast(this->sensor_); - - if (not this->castSensor_) { - std::cerr << "Creating a touch sensor with a non-contact sensor object." << std::endl; - throw std::runtime_error("Sensor error"); - } + namespace gazebo + { + TouchSensor::TouchSensor(::gazebo::physics::ModelPtr model, + sdf::ElementPtr sensor, + std::string partId, + std::string sensorId) + : Sensor(model, sensor, partId, sensorId, 1) + , lastValue_(false) + { + this->castSensor_ = + boost::dynamic_pointer_cast< gz::sensors::ContactSensor >( + this->sensor_); - // Sensor must always update - this->castSensor_->SetActive(true); + if (not this->castSensor_) + { + std::cerr << "Creating a touch sensor with a non-contact sensor object." + << std::endl; + throw std::runtime_error("Sensor error"); + } - // Add update connection that will produce new value - this->updateConnection_ = this->sensor_->ConnectUpdated(boost::bind(&TouchSensor::OnUpdate, this)); -} + // Sensor must always update + this->castSensor_->SetActive(true); -TouchSensor::~TouchSensor() -{} + // Add update connection that will produce new value + this->updateConnection_ = + this->sensor_->ConnectUpdated(boost::bind(&TouchSensor::OnUpdate, + this)); + } -void TouchSensor::OnUpdate() { - auto contacts = this->castSensor_->GetContacts(); - this->lastValue_ = contacts.contact_size() > 0; -} + TouchSensor::~TouchSensor() + {} -void TouchSensor::read(double * input) { - input[0] = lastValue_ ? 1 : 0; -} + void TouchSensor::OnUpdate() + { + auto contacts = this->castSensor_->GetContacts(); + this->lastValue_ = contacts.contact_size() > 0; + } -} /* namespace gazebo */ -} /* namespace revolve */ + void TouchSensor::read(double *input) + { + input[0] = lastValue_ ? 1 : 0; + } + } /* namespace gazebo */ +} /* namespace revolve */ diff --git a/cpp/revolve/gazebo/sensors/VirtualSensor.cpp b/cpp/revolve/gazebo/sensors/VirtualSensor.cpp index cc97b95210..f5b775d613 100644 --- a/cpp/revolve/gazebo/sensors/VirtualSensor.cpp +++ b/cpp/revolve/gazebo/sensors/VirtualSensor.cpp @@ -17,31 +17,40 @@ * */ -#include - -namespace revolve { -namespace gazebo { - -VirtualSensor::VirtualSensor(::gazebo::physics::ModelPtr model, std::string partId, std::string sensorId, unsigned int inputs): - model_(model), - partId_(partId), - sensorId_(sensorId), - inputs_(inputs) -{} - -VirtualSensor::~VirtualSensor() {} +#include -unsigned int VirtualSensor::inputs() { - return inputs_; -} - -std::string VirtualSensor::partId() { - return partId_; -} - -std::string VirtualSensor::sensorId() { - return sensorId_; -} +#include -} /* namespace gazebo */ -} /* namespace revolve */ +namespace revolve +{ + namespace gazebo + { + VirtualSensor::VirtualSensor(::gazebo::physics::ModelPtr model, + std::string partId, + std::string sensorId, + unsigned int inputs) + : model_(model) + , partId_(partId) + , sensorId_(sensorId) + , inputs_(inputs) + {} + + VirtualSensor::~VirtualSensor() + {} + + unsigned int VirtualSensor::inputs() + { + return inputs_; + } + + std::string VirtualSensor::partId() + { + return partId_; + } + + std::string VirtualSensor::sensorId() + { + return sensorId_; + } + } /* namespace gazebo */ +} /* namespace revolve */ diff --git a/cpp/revolve/gazebo/standalone/body_analyzer_server.cpp b/cpp/revolve/gazebo/standalone/body_analyzer_server.cpp index 3771227709..278e187352 100644 --- a/cpp/revolve/gazebo/standalone/body_analyzer_server.cpp +++ b/cpp/revolve/gazebo/standalone/body_analyzer_server.cpp @@ -23,15 +23,17 @@ */ #include "gazebo/common/Exception.hh" -//#include "gazebo/util/LogRecord.hh" +// #include "gazebo/util/LogRecord.hh" #include "gazebo/common/Console.hh" #include "gazebo/Server.hh" ////////////////////////////////////////////////// -int main(int argc, char **argv) +int main(int argc, + char **argv) { char *analyzerLoaded = getenv("ANALYZER_TOOL"); - if (not analyzerLoaded) { + if (not analyzerLoaded) + { std::cerr << "Please run using tools/run-analyzer.sh" << std::endl; return -1; } @@ -42,23 +44,24 @@ int main(int argc, char **argv) try { - // Initialize the informational logger. This will log warnings, and - // errors. + // Initialize the informational logger. This will log warnings, and errors. gzLogInit("server-analyzer-", "gzserver_analyzer.log"); // Initialize the data logger. This will log state information. -// gazebo::util::LogRecord::Instance()->Init("gzserver"); + // gazebo::util::LogRecord::Instance()->Init("gzserver"); server = new gazebo::Server(); if (not server->ParseArgs(argc, argv)) + { return -1; + } server->Run(); server->Fini(); delete server; } - catch(gazebo::common::Exception &_e) + catch (gazebo::common::Exception &_e) { _e.Print();