| 150 | } |
| 151 | |
| 152 | void vizTreeModels(const std::vector<Cylinder> &scanTm, |
| 153 | visualization_msgs::MarkerArray &tMarkerArray, |
| 154 | size_t &cylinderId) |
| 155 | { |
| 156 | for (const auto &tree : scanTm) |
| 157 | { |
| 158 | |
| 159 | Scalar maxTreeRadius = 0.25; |
| 160 | Scalar maxAxisTheta = 45; |
| 161 | visualization_msgs::Marker marker; |
| 162 | marker.header.frame_id = "quadrotor/map"; |
| 163 | // marker.header.frame_id = "quadrotor/map"; |
| 164 | marker.header.stamp = ros::Time(); |
| 165 | marker.id = cylinderId; |
| 166 | marker.type = visualization_msgs::Marker::CYLINDER; |
| 167 | marker.action = visualization_msgs::Marker::ADD; |
| 168 | |
| 169 | // Center of cylinder |
| 170 | marker.pose.position.x = tree.model.root[0] + 0.5 * tree.model.ray[0]; |
| 171 | marker.pose.position.y = tree.model.root[1] + 0.5 * tree.model.ray[1]; |
| 172 | marker.pose.position.z = tree.model.root[2] + 0.5 * tree.model.ray[1]; |
| 173 | // marker.pose.position.z = 0; |
| 174 | |
| 175 | // Orientation of cylidner |
| 176 | Vector3 src_vec(0, 0, 1); |
| 177 | Quat q_rot = Quat::FromTwoVectors(src_vec, tree.model.ray); |
| 178 | marker.pose.orientation.x = q_rot.x(); |
| 179 | marker.pose.orientation.y = q_rot.y(); |
| 180 | marker.pose.orientation.z = q_rot.z(); |
| 181 | marker.pose.orientation.w = q_rot.w(); |
| 182 | |
| 183 | marker.scale.x = 2 * tree.model.radius; |
| 184 | marker.scale.y = 2 * tree.model.radius; |
| 185 | marker.scale.z = 10; |
| 186 | |
| 187 | if (cylinderId < 200000) |
| 188 | { |
| 189 | marker.color.a = 0.5; |
| 190 | marker.color.r = 1.0; |
| 191 | marker.color.g = 1.0; |
| 192 | marker.color.b = 0.0; |
| 193 | } |
| 194 | else |
| 195 | { |
| 196 | marker.color.a = 0.5; |
| 197 | marker.color.r = 1.0; |
| 198 | marker.color.g = 1.0; |
| 199 | marker.color.b = 1.0; |
| 200 | } |
| 201 | |
| 202 | tMarkerArray.markers.push_back(marker); |
| 203 | cylinderId++; |
| 204 | } |
| 205 | // HACK TO MAKE SURE WE ALWAYS DELETE THE PREVIOUS CYLINDERS |
| 206 | for (auto i = cylinderId; i < cylinderId + 50; ++i) |
| 207 | { |
| 208 | visualization_msgs::Marker marker; |
| 209 | marker.header.frame_id = "quadrotor/map"; |
no outgoing calls
no test coverage detected