From e22b15421d0505709ada35f1b166c013d4a4b208 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Wed, 7 Dec 2022 11:27:58 -0500 Subject: [PATCH] fix dynamic layer bindings and node removal (#74) Co-authored-by: Nathan Hughes (fix) emplace agent nodes correctly when loading json, and fix dynamic node python iter clear dynamic node when removing and set python iter to first valid node --- python/bindings/scene_graph_iterators.h | 9 ++++- src/dynamic_scene_graph_layer.cpp | 1 + src/graph_json_serialization.cpp | 50 +++++++++++++++++-------- 3 files changed, 43 insertions(+), 17 deletions(-) diff --git a/python/bindings/scene_graph_iterators.h b/python/bindings/scene_graph_iterators.h index d88f8b0..02b4352 100644 --- a/python/bindings/scene_graph_iterators.h +++ b/python/bindings/scene_graph_iterators.h @@ -61,12 +61,19 @@ class NodeIter { class DynamicNodeIter { public: DynamicNodeIter(const DynamicSceneGraphLayer::Nodes& container) - : curr_iter_(container.begin()), end_iter_(container.end()) {} + : curr_iter_(container.begin()), end_iter_(container.end()) { + while (*curr_iter_ == nullptr && curr_iter_ != end_iter_) { + ++curr_iter_; + } + } const DynamicSceneGraphLayer::Node* operator*() const { return curr_iter_->get(); } DynamicNodeIter& operator++() { ++curr_iter_; + while (*curr_iter_ == nullptr && curr_iter_ != end_iter_) { + ++curr_iter_; + } return *this; } diff --git a/src/dynamic_scene_graph_layer.cpp b/src/dynamic_scene_graph_layer.cpp index 61db21e..4aecd6a 100644 --- a/src/dynamic_scene_graph_layer.cpp +++ b/src/dynamic_scene_graph_layer.cpp @@ -268,6 +268,7 @@ bool DynamicSceneGraphLayer::removeNode(NodeId node) { // TODO(nathan) this is slightly brittle, maybe consider std::map instead node_status_[index] = NodeStatus::DELETED; times_.erase(nodes_.at(index)->timestamp.count()); + nodes_[index].reset(); return true; } diff --git a/src/graph_json_serialization.cpp b/src/graph_json_serialization.cpp index 4d3dda5..9d3f49a 100644 --- a/src/graph_json_serialization.cpp +++ b/src/graph_json_serialization.cpp @@ -55,8 +55,8 @@ using nlohmann::json; using EdgesPtr = std::unique_ptr; using NodeSet = std::unordered_set; using NodeCallback = std::function; -using DynamicNodeCallback = - std::function; +using DynamicNodeCallback = std::function< + void(LayerId, NodeId, std::chrono::nanoseconds, NodeAttributes::Ptr&&)>; using EdgeCallback = std::function; void to_json(json& record, const MeshEdge& edge) { @@ -94,11 +94,12 @@ void read_node_from_json(const json& record, NodeCallback callback) { void read_node_from_json(const json& record, DynamicNodeCallback callback) { auto layer = record.at("layer").get(); - auto prefix = record.at("prefix").get(); + // auto prefix = record.at("prefix").get(); + auto node_id = record.at("id").get(); auto timestamp = record.at("timestamp").get(); JsonConverter converter(&record.at("attributes")); auto attrs = JsonNodeFactory::get_default().create(converter); - callback(layer, prefix, std::chrono::nanoseconds(timestamp), std::move(attrs)); + callback(layer, node_id, std::chrono::nanoseconds(timestamp), std::move(attrs)); } void read_edge_from_json(const json& record, EdgeCallback callback) { @@ -176,7 +177,7 @@ std::string DynamicSceneGraph::serialize(bool include_mesh) const { record["layer_ids"] = layer_ids; record["mesh_layer_id"] = mesh_layer_id; - for (const auto& id_layer_pair : layers_) { + for (const auto& id_layer_pair : layers_){ for (const auto& id_node_pair : id_layer_pair.second->nodes_) { record["nodes"].push_back(*id_node_pair.second); } @@ -198,8 +199,16 @@ std::string DynamicSceneGraph::serialize(bool include_mesh) const { for (const auto& prefix_layer_pair : id_layer_group_pair.second) { const DynamicSceneGraphLayer& layer = *prefix_layer_pair.second; - for (const auto& node : layer.nodes_) { - record["nodes"].push_back(*node); + for (size_t i = 0; i < layer.nodes_.size(); ++i) { + if (!layer.node_status_.count(i)) { + continue; + } + + if (layer.node_status_.at(i) == NodeStatus::DELETED) { + continue; + } + + record["nodes"].push_back(*layer.nodes_.at(i)); } for (const auto& id_edge_pair : layer.edges_.edges) { @@ -251,20 +260,29 @@ DynamicSceneGraph::Ptr DynamicSceneGraph::deserialize(const std::string& content } for (const auto& id_content_pair : dynamic_contents) { - read_node_from_json(id_content_pair.second, - [graph](LayerId layer, - char prefix, - std::chrono::nanoseconds time, - NodeAttributes::Ptr&& attrs) { - graph->emplaceNode( - layer, prefix, time, std::move(attrs), false); - }); + read_node_from_json( + id_content_pair.second, + [graph](LayerId layer, + NodeId node, + std::chrono::nanoseconds time, + NodeAttributes::Ptr&& attrs) { + if (!graph->emplacePrevDynamicNode(layer, node, time, std::move(attrs))) { + std::stringstream ss; + ss << "failed to add " << NodeSymbol(node).getLabel(); + throw std::runtime_error(ss.str()); + } + }); } for (const auto& edge : record.at("edges")) { read_edge_from_json( edge, [graph](NodeId source, NodeId target, EdgeAttributes::Ptr&& attrs) { - graph->insertEdge(source, target, std::move(attrs)); + if (!graph->insertEdge(source, target, std::move(attrs))) { + std::stringstream ss; + ss << "failed to add " << NodeSymbol(source).getLabel() << " → " + << NodeSymbol(target).getLabel(); + throw std::runtime_error(ss.str()); + } }); }