WORKING COMMIT of tgDataManager and related files, including tgSensor…

…Infos! Currently, tgDataManager will create sensors based on the sensorInfos, and everythign 'should' be handled properly with reset, teardown, setup, and various constructors/destructors. There shouldn't be any memory leaks with this commit, I tried very hard to keep it clean!

src/dev/apsabelhaus/tgBoxAnchorDebugDemo/AppTgBoxAnchorDebugDemo.cpp

@@ -34,6 +34,7 @@
 #include "core/tgSimulation.h"
 #include "core/tgWorld.h"
 #include "sensors/tgDataLogger2.h"
+#include "sensors/tgRodSensorInfo.h"
 // Bullet Physics
 #include "LinearMath/btVector3.h"
 // The C++ Standard Library
@@ -88,6 +89,11 @@ int main(int argc, char** argv)
     tgDataLogger2* myDataLogger = new tgDataLogger2(log_filename);
     // Then, add the model to the data logger
     myDataLogger->addSenseable(myModel);
+    // Create sensor infos for all the types of sensors that the data logger
+    // will create.
+    tgRodSensorInfo* myRodSensorInfo = new tgRodSensorInfo();
+    // Attach the sensor infos to the data logger
+    myDataLogger->addSensorInfo(myRodSensorInfo);
     // Next, attach it to the simulation
     simulation.addDataManager(myDataLogger);
     // and everything else should happen automatically.

src/sensors/CMakeLists.txt

@@ -13,4 +13,6 @@ add_library( ${PROJECT_NAME} SHARED
     tgDataLogger2.cpp
     tgSensor.cpp
     tgRodSensor.cpp
+    tgSensorInfo.cpp
+    tgRodSensorInfo.cpp
 )

src/sensors/tgDataLogger2.cpp

@@ -73,8 +73,7 @@ tgDataLogger2::~tgDataLogger2()
  */
 void tgDataLogger2::setup()
 {
-  // Although the parent class doesn't do much in setup at the moment,
-  // call it here anyway, in case that changes!
+  // Call the parent's setup method, which creates the sensors.
   tgDataManager::setup();
   //DEBUGGING
   std::cout << "tgDataLogger2 setup." << std::endl;
@@ -99,8 +98,13 @@ void tgDataLogger2::setup()
       std::cout << descendants[i][j]->getLabelForSensor() << std::endl;
     }
   }
-
-  // TO-DO: setup everything!
+
+  // Check how many sensor infos are attached.
+  //DEBUGGING
+  std::cout << "There are " << m_sensorInfos.size() << " sensor infos attached."
+	    << std::endl;
+  std::cout << "There are " << m_sensors.size() << " sensors attached."
+	    << std::endl;
 
   // Postcondition
   assert(invariant());

src/sensors/tgDataManager.cpp

@@ -28,7 +28,7 @@
 // This application
 #include "tgSensor.h"
 #include "core/tgSenseable.h"
-//#include "tgSensorInfo.h"
+#include "tgSensorInfo.h"
 // The C++ Standard Library
 //#include <stdio.h> // for sprintf
 #include <iostream>
@@ -67,25 +67,67 @@ tgDataManager::~tgDataManager()
     // It is safe to delete NULL, but this is an invariant
     // assert(pChild != NULL); 
     delete pSensor;
+    // Null out the deleted pointer.
+    m_sensors[i] = NULL;
   }
   // Next, delete the sensor infos.
-  // TO-DO: fill in.
+  const size_t n_SensInf = m_sensorInfos.size();
+  for (size_t i = 0; i < n_SensInf; ++i)
+  {
+    // Pick out one sensorInfo from the list:
+    tgSensorInfo* const pSensorInfo = m_sensorInfos[i];
+    // TO-DO: check if we need this assert...
+    // It is safe to delete NULL, but this is an invariant
+    // assert(pChild != NULL); 
+    delete pSensorInfo;
+    //Null out the deleted pointer.
+    m_sensorInfos[i] = NULL;
+  }
 
   // Note that the creation and deletion of the senseable objects, e.g.
   // the tgModels, is handled externally.
   // tgDataManagers should NOT destroy the objects they are sensing.
 }
 
 /**
- * In child classes, setup will probably do something interesting.
- * In the base class, nothing is being created.
+ * Helper for setup.
+ * This function abstracts away the loop over the sensor infos list.
+ */
+void tgDataManager::addSensorsIfAppropriate(tgSenseable* pSenseable)
+{
+  // Loop over all tgSensorInfos in the list.
+  for (size_t i=0; i < m_sensorInfos.size(); i++){
+    // If this particular sensor info is appropriate for the pSenseable,
+    if( m_sensorInfos[i]->isThisMySenseable(pSenseable) ) {
+      // Create a sensor and push it back to our list of sensors.
+      m_sensors.push_back( m_sensorInfos[i]->createSensor(pSenseable));
+    }
+  }
+}
+
+/**
+ * Create the sensors!
+ * Note that it is up to the child classes to do any other type of setup besides
+ * sensor creation. For example, a data logger will have to first call this parent
+ * function, then ask for the sensor heading from each sensor, then... etc.
  */
 void tgDataManager::setup()
 {
-  //DEBUGGING
-  //std::cout << "tgDataManager setup." << std::endl;
-  // TO-DO: Should we create the sensors here in the setup method of the base?
-  // What's the best way to maximize code re-use?
+  // Iterate over all sensables and their descendants, adding sensors
+  // if appropriate.
+  for (size_t j=0; j < m_senseables.size(); j++){
+    // For each senseable object, create sensors for it and its descendants.
+    // First, the senseable itself:
+    addSensorsIfAppropriate(m_senseables[j]);
+    // Then, for all its descendants:
+    std::vector<tgSenseable*> descendants =
+      m_senseables[j]->getSenseableDescendants();
+    // Loop through.
+    for (size_t k=0; k < descendants.size(); k++) {
+      addSensorsIfAppropriate(descendants[k]);
+    }
+  }
+
   // Postcondition
   assert(invariant());
 }
@@ -109,13 +151,16 @@ void tgDataManager::teardown()
     // Note that sensors don't have any teardown method.
     // The delete method will call their destructors.
     delete m_sensors[i];
+    m_sensors[i] = NULL;
   }
   // Clear the list so that the destructor for this class doesn't have to
   // do anything.
   m_sensors.clear();
 
   // Next, delete the sensor infos.
   // TO-DO: implement this.
+  // TO-DO: should we actually be deleting these? Or would that make it so no
+  // sensors would be created after a reset???
 
   // Clear the list of senseable objects.
   // Since tgDataManagers don't ever change these objects,
@@ -158,9 +203,15 @@ void tgDataManager::step(double dt)
  * It takes in a pointer to a sensor info and pushes it to the
  * current list of sensor infos.
  */
-/*
 void tgDataManager::addSensorInfo(tgSensorInfo* pSensorInfo)
 {
+  // TO-DO:
+  // Check if a type of pSensorInfo is already in the list.
+  // Otherwise, duplicate sensors will be created: e.g., if two tgRodSensorInfos
+  // are in m_sensorInfos, then multiple tgRodSensors will be created for
+  // each tgRod.
+  //DEBUGGING
+  std::cout << "tgDataManager addSensorInfo." << std::endl;
   // Precondition
   if (pSensorInfo == NULL)
   {
@@ -173,7 +224,6 @@ void tgDataManager::addSensorInfo(tgSensorInfo* pSensorInfo)
   assert(invariant());
   assert(!m_sensorInfos.empty());
 }
-*/
 
 /**
  * This method adds sense-able objects to this data manager.

src/sensors/tgDataManager.h

@@ -41,7 +41,7 @@
 //class tgWorld;
 class tgSensor;
 //class tgSenseable;
-//class tgSensorInfo;
+class tgSensorInfo;
 
 /**
  * Abstract class for objects that will manage data within NTRTsim.
@@ -96,7 +96,7 @@ class tgDataManager
      * Add a sensor info object to the current list of sensor infos.
      * @param[in] pSensorInfo a pointer to a tgSensorInfo.
      */
-    //virtual void addSensorInfo(tgSensorInfo* pSensorInfo);
+    virtual void addSensorInfo(tgSensorInfo* pSensorInfo);
 
     /**
      * Returns some basic information about this tgDataManager,
@@ -106,6 +106,14 @@ class tgDataManager
      */
     virtual std::string toString() const;
 
+ private:
+
+    /**
+     * A helper function for setup. Since there will be a loop over
+     * the sensor infos, this function abstracts it away.
+     */
+    void addSensorsIfAppropriate(tgSenseable* pSenseable);
+
 protected:
 
     /** Integrity predicate. */
@@ -122,7 +130,7 @@ class tgDataManager
      * have been passed in to it.
      * These are used to create the sensors
      */
-    //std::vector<tgSensorInfo*> m_sensorInfos;
+    std::vector<tgSensorInfo*> m_sensorInfos;
 
     /**
      * A data manager will also have a list of tgSenseable objects 

src/sensors/tgRodSensor.cpp

@@ -47,6 +47,8 @@
  */
 tgRodSensor::tgRodSensor(tgRod* pRod) : tgSensor(pRod)
 {
+  // Note that this pointer may be 0 (equivalent to NULL) if the cast in
+  // the calling function from tgSenseable to tgRod fails.
   if (pRod == NULL) {
     throw std::invalid_argument("Pointer to pRod is NULL inside tgRodSensor.");
   }

src/sensors/tgRodSensorInfo.cpp

@@ -0,0 +1,77 @@
+/*
+ * Copyright © 2012, United States Government, as represented by the
+ * Administrator of the National Aeronautics and Space Administration.
+ * All rights reserved.
+ * 
+ * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
+ * under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with 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.
+*/
+
+/**
+ * @file tgRodSensorInfo.cpp
+ * @brief Contains the implementation of concrete class tgRodSensorInfo
+ * @author Drew Sabelhaus
+ * $Id$
+ */
+
+// This module
+#include "tgRodSensorInfo.h"
+// Other includes from NTRTsim
+#include "tgRodSensor.h"
+#include "core/tgRod.h"
+#include "core/tgSenseable.h"
+#include "core/tgCast.h"
+// Other includes from the C++ standard library
+#include <stdexcept>
+
+/**
+ * Nothing to do in this constructor. A sensor info doesn't have any data.
+ */
+tgRodSensorInfo::tgRodSensorInfo()
+{
+}
+
+/**
+ * Similarly, empty destructor.
+ */
+tgRodSensorInfo::~tgRodSensorInfo()
+{
+}
+
+/**
+ * To check if a tgSenseable is a tgRod, we can use tgCast.
+ */
+bool tgRodSensorInfo::isThisMySenseable(tgSenseable* pSenseable)
+{
+  // The following cast will return 0 if the senseable is not a tgRod.
+  tgRod* m_pRod = tgCast::cast<tgSenseable, tgRod>(pSenseable);
+  if( m_pRod == 0 )
+    return 0;
+  else {
+    return 1;
+  }
+}
+
+/**
+ * Create a rod sensor for a tgRod.
+ */
+tgSensor* tgRodSensorInfo::createSensor(tgSenseable* pSenseable)
+{
+  //CHECK: the caller SHOULD HAVE made sure that pSenseable
+  // was a tgRod pointer. If not, complain!!
+  if (!isThisMySenseable(pSenseable)) {
+    throw std::invalid_argument("pSenseable is NOT a tgRod, inside tgRodSensorInfo.");
+  }
+  // Then, if the program hasn't quit, make the sensor.
+  // Note that we cast the pointer here, knowing that it will succeed.
+  return new tgRodSensor( tgCast::cast<tgSenseable, tgRod>(pSenseable) );
+}

src/sensors/tgRodSensorInfo.h

@@ -0,0 +1,75 @@
+/*
+ * Copyright © 2012, United States Government, as represented by the
+ * Administrator of the National Aeronautics and Space Administration.
+ * All rights reserved.
+ * 
+ * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
+ * under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with 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.
+*/
+
+#ifndef TG_ROD_SENSOR_INFO_H
+#define TG_ROD_SENSOR_INFO_H
+
+/**
+ * @file tgRodSensorInfo.h
+ * @brief Definition of concrete class tgRodSensorInfo 
+ * @author Drew Sabelhaus
+ * @date January 2017
+ * $Id$
+ */
+
+// This module
+#include "tgSensorInfo.h"
+// Other includes from NTRTsim
+//#include "core/tgRod.h"
+// Other includes from the C++ standard library
+// ...
+
+// Forward references
+class tgSenseable;
+class tgRod;
+class tgRodSensor;
+class tgSensor;
+
+/**
+ * tgRodSensorInfo is a sensor info class that creates tgRodSensors for
+ * tgRods.
+ */
+class tgRodSensorInfo : public tgSensorInfo
+{
+ public:
+
+  /**
+   * tgSensorInfo has an empty constructor. It does not contain any data.
+   */
+  tgRodSensorInfo();
+
+  /** 
+   * Similarly, its destructor should be empty.
+   */
+  ~tgRodSensorInfo();
+
+  /**
+   * From tgSensorInfo, need to implement a check to see if a particular
+   * tgSenseable is a tgRod.
+   */
+  virtual bool isThisMySenseable(tgSenseable* pSenseable);
+
+  /**
+   * Similarly, create a sensor if appropriate.
+   * See tgSensorInfo for more... info.
+   */
+  virtual tgSensor* createSensor(tgSenseable* pSenseable);
+
+};
+
+#endif // TG_ROD_SENSOR_INFO_H