[Sensor] SensorSuite in each SceneNode

habitat_sim/__init__.py

@@ -59,6 +59,7 @@
         SceneNode,
         SceneNodeType,
         Sensor,
+        SensorFactory,
         SensorSpec,
         SensorSubType,
         SensorType,

habitat_sim/agent/agent.py

@@ -179,18 +179,8 @@ def _add_sensor(
         if modify_agent_config:
             assert spec not in self.agent_config.sensor_specifications
             self.agent_config.sensor_specifications.append(spec)
-        if not spec.is_visual_sensor_spec:
-            raise ValueError(
-                f"""{spec.sensor_type} is a sensorType that is not implemented yet"""
-            )
-        CameraSensorSubTypeSet = {
-            habitat_sim.SensorSubType.PINHOLE,
-            habitat_sim.SensorSubType.ORTHOGRAPHIC,
-        }
-        if spec.sensor_subtype not in CameraSensorSubTypeSet:
-            raise ValueError(f"""{spec.sensor_subtype} is an illegal sensorSubType""")
-        # TODO: Add more checks for NonVisualSensorSpec, Other types of sensors
-        self._sensors.add(hsim.CameraSensor(self.scene_node.create_child(), spec))
+        sensor_suite = hsim.SensorFactory.create_sensors(self.scene_node, [spec])
+        self._sensors.add(sensor_suite[spec.uuid])
 
     def act(self, action_id: Any) -> bool:
         r"""Take the action specified by action_id

habitat_sim/bindings/__init__.py

@@ -20,6 +20,7 @@
     SceneNode,
     SceneNodeType,
     Sensor,
+    SensorFactory,
     SensorSpec,
     SensorSubType,
     SensorType,

src/esp/agent/Agent.cpp

@@ -22,14 +22,12 @@ const std::set<std::string> Agent::BodyActions = {"moveRight",   "moveLeft",
 Agent::Agent(scene::SceneNode& agentNode, const AgentConfiguration& cfg)
     : Magnum::SceneGraph::AbstractFeature3D(agentNode),
       configuration_(cfg),
-      sensors_(),
       controls_(scene::ObjectControls::create()) {
   agentNode.setType(scene::SceneNodeType::AGENT);
 }  // Agent::Agent
 
 Agent::~Agent() {
   LOG(INFO) << "Deconstructing Agent";
-  sensors_.clear();
 }
 
 bool Agent::act(const std::string& actionName) {
@@ -40,8 +38,8 @@ bool Agent::act(const std::string& actionName) {
                         actionSpec.actuation.at("amount"),
                         /*applyFilter=*/true);
     } else {
-      for (const auto& p : sensors_.getSensors()) {
-        controls_->action(p.second->object(), actionSpec.name,
+      for (const auto& p : node().getNodeSensors()) {
+        controls_->action(p.second.get().object(), actionSpec.name,
                           actionSpec.actuation.at("amount"),
                           /*applyFilter=*/false);
       }
@@ -79,8 +77,8 @@ void Agent::setState(const AgentState& state,
   node().setRotation(Magnum::Quaternion(quatf(rot)).normalized());
 
   if (resetSensors) {
-    for (const auto& p : sensors_.getSensors()) {
-      p.second->setTransformationFromSpec();
+    for (auto& p : node().getNodeSensors()) {
+      p.second.get().setTransformationFromSpec();
     }
   }
   // TODO other state members when implemented

src/esp/agent/Agent.h

@@ -14,6 +14,10 @@
 #include "esp/sensor/Sensor.h"
 
 namespace esp {
+
+namespace sensor {
+class SensorSuite;
+}
 namespace agent {
 
 // Represents the physical state of an agent
@@ -111,11 +115,6 @@ class Agent : public Magnum::SceneGraph::AbstractFeature3D {
 
   void setState(const AgentState& state, const bool resetSensors = true);
 
-  // Set Agent's member sensors to sensorSuite
-  void setSensorSuite(sensor::SensorSuite&& sensorSuite) {
-    sensors_ = sensorSuite;
-  }
-
   void setInitialState(const AgentState& state,
                        const bool resetSensors = true) {
     initialState_ = state;
@@ -124,8 +123,24 @@ class Agent : public Magnum::SceneGraph::AbstractFeature3D {
 
   scene::ObjectControls::ptr getControls() { return controls_; }
 
-  const sensor::SensorSuite& getSensorSuite() const { return sensors_; }
-  sensor::SensorSuite& getSensorSuite() { return sensors_; }
+  /**
+   * @brief Return SensorSuite containing references to superset of all Sensors
+   * held by this Agent's SceneNode and its children
+   */
+  sensor::SensorSuite& getSubtreeSensorSuite() {
+    return node().getSubtreeSensorSuite();
+  }
+
+  /**
+   * @brief Return map containing references to superset of all Sensors held by
+   * this Agent's SceneNode and its children values.
+   * Keys of map are uuid strings, values are references to Sensors with that
+   * uuid
+   */
+  std::map<std::string, std::reference_wrapper<sensor::Sensor>>&
+  getSubtreeSensors() {
+    return node().getSubtreeSensors();
+  }
 
   const AgentConfiguration& getConfig() const { return configuration_; }
   AgentConfiguration& getConfig() { return configuration_; }
@@ -138,7 +153,6 @@ class Agent : public Magnum::SceneGraph::AbstractFeature3D {
 
  private:
   AgentConfiguration configuration_;
-  sensor::SensorSuite sensors_;
   scene::ObjectControls::ptr controls_;
   AgentState initialState_;
 

src/esp/bindings/SceneBindings.cpp

@@ -58,6 +58,8 @@ void initSceneBindings(py::module& m) {
       .def(
           "create_child", [](SceneNode& self) { return &self.createChild(); },
           R"(Creates a child node, and sets its parent to the current node.)")
+      .def("set_parent", &SceneNode::setParent,
+           R"(Sets parent to parentNode, and updates ancestors' SensorSuites)")
       .def(
           "compute_cumulative_bb", &SceneNode::computeCumulativeBB,
           R"(Recursively compute the approximate axis aligned bounding boxes of the SceneGraph sub-tree rooted at this node.)")
@@ -72,7 +74,17 @@ void initSceneBindings(py::module& m) {
           py::overload_cast<>(&SceneNode::absoluteTranslation))
       .def_property_readonly(
           "absolute_translation",
-          py::overload_cast<>(&SceneNode::absoluteTranslation, py::const_));
+          py::overload_cast<>(&SceneNode::absoluteTranslation, py::const_))
+      .def_property_readonly("node_sensor_suite",
+                             &SceneNode::getNodeSensorSuite,
+                             R"(Get node SensorSuite of this SceneNode)")
+      .def_property_readonly("subtree_sensor_suite",
+                             &SceneNode::getSubtreeSensorSuite,
+                             R"(Get subtree SensorSuite of this SceneNode)")
+      .def_property_readonly("node_sensors", &SceneNode::getNodeSensors,
+                             R"(Get node sensors of this SceneNode)")
+      .def_property_readonly("subtree_sensors", &SceneNode::getSubtreeSensors,
+                             R"(Get subtree sensors of this SceneNode)");
 
   py::class_<SceneGraph>(m, "SceneGraph")
       .def(py::init())

src/esp/bindings/SensorBindings.cpp

@@ -18,6 +18,7 @@
 #include "esp/sensor/RedwoodNoiseModel.h"
 #endif
 #include "esp/sensor/Sensor.h"
+#include "esp/sensor/SensorFactory.h"
 #include "esp/sim/Simulator.h"
 
 namespace py = pybind11;
@@ -96,6 +97,28 @@ void initSensorBindings(py::module& m) {
       .def(py::init(&CameraSensorSpec::create<>))
       .def_readwrite("ortho_scale", &CameraSensorSpec::orthoScale);
 
+  // ==== SensorFactory ====
+  py::class_<SensorFactory>(m, "SensorFactory")
+      .def("create_sensors", &SensorFactory::createSensors)
+      .def("delete_sensor", &SensorFactory::deleteSensor)
+      .def("delete_subtree_sensor", &SensorFactory::deleteSubtreeSensor);
+
+  // ==== SensorSuite ====
+  py::class_<SensorSuite, Magnum::SceneGraph::PyFeature<SensorSuite>,
+             Magnum::SceneGraph::AbstractFeature3D,
+             Magnum::SceneGraph::PyFeatureHolder<SensorSuite>>(m, "SensorSuite")
+      .def("add", &SensorSuite::add)
+      .def("remove", py::overload_cast<const Sensor&>(&SensorSuite::remove))
+      .def("remove",
+           py::overload_cast<const std::string&>(&SensorSuite::remove))
+      .def("clear", &SensorSuite::clear)
+      .def("get", &SensorSuite::get)
+      .def("get_sensors",
+           py::overload_cast<>(&SensorSuite::getSensors, py::const_))
+      .def_property_readonly("node", nodeGetter<Sensor>,
+                             "Node this object is attached to")
+      .def_property_readonly("object", nodeGetter<Sensor>, "Alias to node");
+
   // ==== Sensor ====
   py::class_<Sensor, Magnum::SceneGraph::PyFeature<Sensor>,
              Magnum::SceneGraph::AbstractFeature3D,

src/esp/bindings_js/bindings_js.cpp

@@ -51,6 +51,15 @@ std::map<std::string, ObservationSpace> Simulator_getAgentObservationSpaces(
   return spaces;
 }
 
+std::map<std::string, Sensor::ptr> Agent_getSubtreeSensors(Agent& agent) {
+  std::map<std::string, Sensor::ptr> jsSensors =
+      std::map<std::string, Sensor::ptr>();
+  for (auto& entry : agent.node().getSubtreeSensors()) {
+    jsSensors[entry.first] = std::shared_ptr<Sensor>(&entry.second.get());
+  }
+  return jsSensors;
+}
+
 template <class T, typename... Targs>
 static inline auto create(Targs&&... args) {
   return std::make_shared<T>(std::forward<Targs>(args)...);
@@ -102,7 +111,6 @@ EMSCRIPTEN_BINDINGS(habitat_sim_bindings_js) {
   em::register_vector<std::shared_ptr<SemanticCategory>>(
       "VectorSemanticCategories");
   em::register_vector<std::shared_ptr<SemanticObject>>("VectorSemanticObjects");
-
   em::register_map<std::string, float>("MapStringFloat");
   em::register_map<std::string, std::string>("MapStringString");
   em::register_map<std::string, Sensor::ptr>("MapStringSensor");
@@ -192,10 +200,6 @@ EMSCRIPTEN_BINDINGS(habitat_sim_bindings_js) {
       .property("bounds", &PathFinder::bounds)
       .function("isNavigable", &PathFinder::isNavigable);
 
-  em::class_<SensorSuite>("SensorSuite")
-      .smart_ptr_constructor("SensorSuite", &SensorSuite::create<>)
-      .function("get", &SensorSuite::get);
-
   em::enum_<SensorType>("SensorType")
       .value("NONE", SensorType::None)
       .value("COLOR", SensorType::Color)
@@ -212,6 +216,7 @@ EMSCRIPTEN_BINDINGS(habitat_sim_bindings_js) {
       .value("NONE", SensorSubType::None)
       .value("PINHOLE", SensorSubType::Pinhole)
       .value("ORTHOGRAPHIC", SensorSubType::Orthographic);
+
   em::class_<SensorSpec>("SensorSpec")
       .smart_ptr_constructor("SensorSpec", &SensorSpec::create<>)
       .property("uuid", &SensorSpec::uuid)
@@ -261,12 +266,11 @@ EMSCRIPTEN_BINDINGS(habitat_sim_bindings_js) {
       .property("config",
                 em::select_overload<const AgentConfiguration&() const>(
                     &Agent::getConfig))
-      .property("sensorSuite", em::select_overload<const SensorSuite&() const>(
-                                   &Agent::getSensorSuite))
       .function("getState", &Agent::getState)
       .function("setState", &Agent::setState)
       .function("hasAction", &Agent::hasAction)
-      .function("act", &Agent::act);
+      .function("act", &Agent::act)
+      .function("getSubtreeSensors", &Agent_getSubtreeSensors);
 
   em::class_<Observation>("Observation")
       .smart_ptr_constructor("Observation", &Observation::create<>)

src/esp/bindings_js/modules/vr_demo.js

@@ -68,7 +68,7 @@ class VRDemo extends WebDemo {
 
     this.sensorSpecs = new Array(VIEW_SENSORS.length);
     for (var iView = 0; iView < VIEW_SENSORS.length; ++iView) {
-      const sensor = agent.sensorSuite.get(VIEW_SENSORS[iView]);
+      const sensor = agent.getSubtreeSensors().get(VIEW_SENSORS[iView]);
 
       this.sensorSpecs[iView] = sensor.specification();
     }
@@ -351,7 +351,7 @@ class VRDemo extends WebDemo {
     for (var iView = 0; iView < pose.views.length; ++iView) {
       const view = pose.views[iView];
 
-      const sensor = agent.sensorSuite.get(VIEW_SENSORS[iView]);
+      const sensor = agent.getSubtreeSensors().get(VIEW_SENSORS[iView]);
 
       const pos = pointToArray(view.transform.position).slice(0, -1); // don't need w for position
       sensor.setLocalTransform(
@@ -393,7 +393,7 @@ class VRDemo extends WebDemo {
       const viewport = layer.getViewport(view);
       this.gl.viewport(viewport.x, viewport.y, viewport.width, viewport.height);
 
-      const sensor = agent.sensorSuite.get(VIEW_SENSORS[iView]);
+      const sensor = agent.getSubtreeSensors().get(VIEW_SENSORS[iView]);
       const texRes = this.sensorSpecs[iView].resolution;
       const texData = sensor.getObservation(this.simenv.sim).getData();
       this.drawTextureData(texRes, texData);