| 128 | } |
| 129 | |
| 130 | void SLOAMNode::publishMap_(const ros::Time stamp) |
| 131 | { |
| 132 | sloam_msgs::ROSObservation obs; |
| 133 | obs.header.stamp = stamp; |
| 134 | obs.header.frame_id = map_frame_id_; |
| 135 | if(pubMapTreeModel_.getNumSubscribers() > 0) |
| 136 | { |
| 137 | auto semantic_map = semanticMap_.getMap(); |
| 138 | // for (const auto &cyl : semantic_map) |
| 139 | // { |
| 140 | // sloam_msgs::ROSCylinder cylMsg; |
| 141 | // cylMsg.id = cyl.id; |
| 142 | // cylMsg.ray = {cyl.model.ray[0], cyl.model.ray[1], cyl.model.ray[2]}; |
| 143 | // cylMsg.root = {cyl.model.root[0], cyl.model.root[1], cyl.model.root[2]}; |
| 144 | // cylMsg.radius = cyl.model.radius; |
| 145 | // cylMsg.radii = cyl.model.radii; |
| 146 | // obs.treeModels.push_back(cylMsg); |
| 147 | // } |
| 148 | // // complete model |
| 149 | // pubObs_.publish(obs); |
| 150 | // viz only |
| 151 | visualization_msgs::MarkerArray mapTMarkerArray; |
| 152 | size_t cid = 500000; |
| 153 | vizTreeModels(semantic_map, mapTMarkerArray, cid); |
| 154 | pubMapTreeModel_.publish(mapTMarkerArray); |
| 155 | } |
| 156 | } |
| 157 | |
| 158 | Cloud::Ptr SLOAMNode::trellisCloud(const std::vector<std::vector<TreeVertex>> &landmarks) |
| 159 | { |
nothing calls this directly
no test coverage detected