1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard 3 // All rights reserved. 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are 7 // met: 8 // 9 // * Redistributions of source code must retain the above copyright notice, 10 // this list of conditions and the following disclaimer. 11 // * Redistributions in binary form must reproduce the above copyright 12 // notice, this list of conditions and the following disclaimer in the 13 // documentation and/or other materials provided with the distribution. 14 // 15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 27 #include "simulator.h" 28 #include <iostream> 29 namespace g2o{ 30 using namespace std; 31 32 // BaseWorldObject ~BaseWorldObject()33 BaseWorldObject::~BaseWorldObject() {} graph()34 OptimizableGraph* BaseWorldObject::graph() { 35 if (_world) 36 return _world-> graph(); 37 return 0; 38 } 39 setVertex(OptimizableGraph::Vertex * vertex_)40 void BaseWorldObject::setVertex(OptimizableGraph::Vertex* vertex_){ 41 _vertex= vertex_; 42 } 43 44 // BaseRobot graph()45 OptimizableGraph* BaseRobot::graph() { 46 if (_world) 47 return _world-> graph(); 48 return 0; 49 } 50 addSensor(BaseSensor * sensor)51 bool BaseRobot::addSensor(BaseSensor* sensor){ 52 assert (graph()); 53 std::pair<std::set<BaseSensor*>::iterator, bool> result=_sensors.insert(sensor); 54 if(result.second){ 55 sensor->setRobot(this); 56 sensor->addParameters(); 57 } 58 return result.second; 59 } 60 sense()61 void BaseRobot::sense() { 62 for (std::set<BaseSensor*>::iterator it=_sensors.begin(); it!=_sensors.end(); ++it){ 63 BaseSensor* s=*it; 64 s->sense(); 65 } 66 } 67 68 // Sensor world()69 World* BaseSensor::world() { 70 if (!_robot) 71 return 0; 72 return _robot->world(); 73 } 74 graph()75 OptimizableGraph* BaseSensor::graph() { 76 if (!_robot) 77 return 0; 78 return _robot->graph(); 79 } 80 81 //World addRobot(BaseRobot * robot)82 bool World::addRobot(BaseRobot* robot){ 83 std::pair<std::set<BaseRobot*>::iterator, bool> result=_robots.insert(robot); 84 if (result.second){ 85 robot->setWorld(this); 86 } 87 return result.second; 88 } 89 addWorldObject(BaseWorldObject * object)90 bool World::addWorldObject(BaseWorldObject* object){ 91 std::pair<std::set<BaseWorldObject*>::iterator, bool> result=_objects.insert(object); 92 if (result.second){ 93 object->setWorld(this); 94 } 95 if (graph() && object->vertex()){ 96 object->vertex()->setId(_runningId++); 97 graph()->addVertex(object->vertex()); 98 } 99 return result.second; 100 } 101 addParameter(Parameter * param)102 bool World::addParameter(Parameter* param){ 103 if ( !graph()) 104 return false; 105 param->setId(_paramId); 106 graph()->addParameter(param); 107 _paramId++; 108 return true; 109 } 110 111 112 } // end namespace 113