void InstrumentRenderer::draw(const std::vector<bool> &visibleComps,
bool showGuides, bool picking) {
const auto &compInfo = m_actor.componentInfo();
std::vector<bool> visited(compInfo.size(), false);
for (size_t i = compInfo.root(); i != std::numeric_limits<size_t>::max();
--i) {
auto type = compInfo.componentType(i);
if (type == ComponentType::Infinite)
continue;
if (type == ComponentType::Grid) {
if (visibleComps[i]) {
drawGridBank(i, picking);
updateVisited(compInfo, i, visited);
}
continue;
}
if (type == ComponentType::Rectangular) {
if (visibleComps[i]) {
drawRectangularBank(i, picking);
updateVisited(compInfo, i, visited);
}
continue;
}
if (type == ComponentType::OutlineComposite) {
if (visibleComps[i]) {
drawTube(i, picking);
updateVisited(compInfo, i, visited);
}
continue;
}
if (type == ComponentType::Structured) {
if (visibleComps[i]) {
drawStructuredBank(i, picking);
updateVisited(compInfo, i, visited);
}
continue;
}
if (!compInfo.isDetector(i) && !showGuides) {
visited[i] = true;
continue;
}
if (compInfo.hasValidShape(i) && visibleComps[i] && !visited[i]) {
visited[i] = true;
drawSingleDetector(i, picking);
}
}
}
void BranchBase::update(vertexName name) {
if(!visited_nodes[name].first) {
unexplored_nodes--;
}
updateVisited(name);
map.updateEdgeCount(getLatestNode()->name_,name);
distance += updateDistance(name);
updateOdom(name);
// double odomUnc = updateOdom(name);
// odom_unc_cost += odomUnc;
std::shared_ptr<Node> node_ptr = std::make_shared<Node>(name,0,distance,visited_nodes[name].first);
node_ptr->stamp = *(timestamps_.rbegin());
node_ptr->position_ = node_list.size();
node_ptr->previous_offset_ = (double)getPreviousDistance(name);
node_list.push_back(node_ptr);
visited_nodes[name].second = node_ptr;
if(odom_unc_cost > max_odom_unc) {
please_kill_me = true;
}
if(max_odom_cost < odom_unc_cost) {
max_odom_cost = odom_unc_cost;
}
}
BranchBase::BranchBase(ros::NodeHandle& n,Graph& graph,vertexName start_vertex): n_(n),map(graph) {
std::shared_ptr<Node> start_node= std::make_shared<Node>(start_vertex,0.0,0.0,0.0);
updateVisited(start_vertex);
node_list.push_back(start_node);
unexplored_nodes = map.size();
//unexplored_cost = unexplored_penality*map.size();
//unexplored_cost = getunexploredcost();
}
BranchBase::BranchBase(ros::NodeHandle& n,Graph& graph,Node& start_node): n_(n),map(graph) {
updateVisited(start_node.name_);
node_list.push_back(std::make_shared<Node>(start_node));
unexplored_nodes = map.size();
// unexplored_cost = unexplored_penality*map.size();
// unexplored_cost = getunexploredcost();
}