| 200 | } |
| 201 | |
| 202 | void GridMapDisplay::processMessage(const grid_map_msgs::GridMap::ConstPtr& msg) |
| 203 | { |
| 204 | // Check if transform between the message's frame and the fixed frame exists. |
| 205 | Ogre::Quaternion orientation; |
| 206 | Ogre::Vector3 position; |
| 207 | if (!context_->getFrameManager()->getTransform(msg->info.header.frame_id, msg->info.header.stamp, |
| 208 | position, orientation)) { |
| 209 | ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->info.header.frame_id.c_str(), |
| 210 | qPrintable(fixed_frame_)); |
| 211 | return; |
| 212 | } |
| 213 | |
| 214 | boost::shared_ptr<GridMapVisual> visual; |
| 215 | if (visuals_.full()) { |
| 216 | visual = visuals_.front(); |
| 217 | } else { |
| 218 | visual.reset(new GridMapVisual(context_->getSceneManager(), scene_node_)); |
| 219 | } |
| 220 | |
| 221 | visual->setMessage(msg); |
| 222 | visual->setFramePosition(position); |
| 223 | visual->setFrameOrientation(orientation); |
| 224 | |
| 225 | visual->computeVisualization(alphaProperty_->getFloat(), showGridLinesProperty_->getBool(), |
| 226 | heightModeProperty_->getOptionInt() == 1, heightTransformerProperty_->getStdString(), |
| 227 | colorModeProperty_->getOptionInt() == 2, colorModeProperty_->getOptionInt() == 3, |
| 228 | colorProperty_->getOgreColor(), colorModeProperty_->getOptionInt() == 1, |
| 229 | colorTransformerProperty_->getStdString(), useRainbowProperty_->getBool(), |
| 230 | invertRainbowProperty_->getBool(), minColorProperty_->getOgreColor(), |
| 231 | maxColorProperty_->getOgreColor(), autocomputeIntensityBoundsProperty_->getBool(), |
| 232 | minIntensityProperty_->getFloat(), maxIntensityProperty_->getFloat()); |
| 233 | |
| 234 | std::vector<std::string> layer_names = visual->getLayerNames(); |
| 235 | heightTransformerProperty_->clearOptions(); |
| 236 | colorTransformerProperty_->clearOptions(); |
| 237 | for (size_t i = 0; i < layer_names.size(); i++) { |
| 238 | heightTransformerProperty_->addOptionStd(layer_names[i]); |
| 239 | colorTransformerProperty_->addOptionStd(layer_names[i]); |
| 240 | } |
| 241 | |
| 242 | visuals_.push_back(visual); |
| 243 | } |
| 244 | |
| 245 | } // end namespace grid_map_rviz_plugin |
| 246 |
nothing calls this directly
no test coverage detected