Simplified with built-in grid map methods and reformatted code.

grisrobotics · Aug 19, 2016 · ea4a343 · ea4a343
1 parent d5e068d
commit ea4a343
Show file tree

Hide file tree

Showing 6 changed files with 437 additions and 504 deletions.
diff --git a/grid_map_rviz_plugin/CMakeLists.txt b/grid_map_rviz_plugin/CMakeLists.txt
@@ -6,6 +6,7 @@ set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
 find_package(catkin REQUIRED COMPONENTS
 	rviz
 	grid_map_ros
+	grid_map_msgs
 )
 
 catkin_package(
@@ -16,6 +17,7 @@ catkin_package(
   CATKIN_DEPENDS
 		rviz
 		grid_map_ros
+		grid_map_msgs
   DEPENDS
 )
 

diff --git a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapDisplay.hpp b/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapDisplay.hpp
@@ -2,7 +2,7 @@
  * GridMapDisplay.h
  *
  *  Created on: Aug 3, 2016
- *  Author: Philipp Krüsi
+ *  Author: Philipp Krüsi, Péter Fankhauser
  *  Institute: ETH Zurich, Autonomous Systems Lab
  */
 
@@ -16,69 +16,67 @@
 
 #include <grid_map_msgs/GridMap.h>
 
-namespace Ogre
-{
-  class SceneNode;
+namespace Ogre {
+class SceneNode;
 }
 
-namespace rviz
-{
-  class BoolProperty;
-  class ColorProperty;
-  class FloatProperty;
-  class IntProperty;
-  class EnumProperty;
-  class EditableEnumProperty;
+namespace rviz {
+class BoolProperty;
+class ColorProperty;
+class FloatProperty;
+class IntProperty;
+class EnumProperty;
+class EditableEnumProperty;
 }
 
-namespace grid_map_rviz_plugin
+namespace grid_map_rviz_plugin {
+class GridMapVisual;
+class GridMapDisplay : public MessageFilterDisplayMod<grid_map_msgs::GridMap,
+    grid_map_msgs::GridMapInfo>
 {
-  class GridMapVisual;
-  class GridMapDisplay: public MessageFilterDisplayMod<grid_map_msgs::GridMap, grid_map_msgs::GridMapInfo>
-  {
-  Q_OBJECT
-  public:
-    // Constructor
-    GridMapDisplay();
-    // Destructor
-    virtual ~GridMapDisplay();
+Q_OBJECT
+ public:
+  // Constructor
+  GridMapDisplay();
+  // Destructor
+  virtual ~GridMapDisplay();
 
-  protected:
-    virtual void onInitialize();
+ protected:
+  virtual void onInitialize();
 
-    virtual void reset();
+  virtual void reset();
 
-  private Q_SLOTS:
-    // Qt slots
-    void updateHistoryLength();
-    void updateHeightMode();
-    void updateColorMode();
-    void updateUseRainbow();
-    void updateAutocomputeIntensityBounds();
-    void updateVisualization();
+ private Q_SLOTS:
+  // Qt slots
+  void updateHistoryLength();
+  void updateHeightMode();
+  void updateColorMode();
+  void updateUseRainbow();
+  void updateAutocomputeIntensityBounds();
+  void updateVisualization();
 
-  private:
-    // Callback for incoming ROS messages
-    void processMessage(const grid_map_msgs::GridMap::ConstPtr& msg);
+ private:
+  // Callback for incoming ROS messages
+  void processMessage(const grid_map_msgs::GridMap::ConstPtr& msg);
 
-    // Circular buffer for visuals
-    boost::circular_buffer<boost::shared_ptr<GridMapVisual> > visuals_;
+  // Circular buffer for visuals
+  boost::circular_buffer<boost::shared_ptr<GridMapVisual> > visuals_;
 
-    // Property variables
-    rviz::FloatProperty* alpha_property_;
-    rviz::IntProperty* history_length_property_;
-    rviz::BoolProperty* show_grid_lines_property_;
-    rviz::EnumProperty* height_mode_property_;
-    rviz::EditableEnumProperty* height_transformer_property_;
-    rviz::EnumProperty* color_mode_property_;
-    rviz::EditableEnumProperty* color_transformer_property_;
-    rviz::ColorProperty* color_property_;
-    rviz::BoolProperty* use_rainbow_property_;
-    rviz::ColorProperty* min_color_property_;
-    rviz::ColorProperty* max_color_property_;
-    rviz::BoolProperty* autocompute_intensity_bounds_property_;
-    rviz::FloatProperty* min_intensity_property_;
-    rviz::FloatProperty* max_intensity_property_;
-  };
+  // Property variables
+  rviz::FloatProperty* alphaProperty_;
+  rviz::IntProperty* historyLengthProperty_;
+  rviz::BoolProperty* showGridLinesProperty_;
+  rviz::EnumProperty* heightModeProperty_;
+  rviz::EditableEnumProperty* heightTransformerProperty_;
+  rviz::EnumProperty* colorModeProperty_;
+  rviz::EditableEnumProperty* colorTransformerProperty_;
+  rviz::ColorProperty* colorProperty_;
+  rviz::BoolProperty* useRainbowProperty_;
+  rviz::ColorProperty* minColorProperty_;
+  rviz::ColorProperty* maxColorProperty_;
+  rviz::BoolProperty* autocomputeIntensityBoundsProperty_;
+  rviz::FloatProperty* minIntensityProperty_;
+  rviz::FloatProperty* maxIntensityProperty_;
+};
 
-} // end namespace grid_map_rviz_plugin
+}  // end namespace grid_map_rviz_plugin
diff --git a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapVisual.hpp b/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapVisual.hpp
@@ -2,7 +2,7 @@
  * GridMapVisual.cpp
  *
  *  Created on: Aug 3, 2016
- *  Author: Philipp Krüsi
+ *  Author: Philipp Krüsi, Péter Fankhauser
  *  Institute: ETH Zurich, Autonomous Systems Lab
  */
 
@@ -14,77 +14,63 @@
 #include <grid_map_msgs/GridMap.h>
 #include <grid_map_core/GridMap.hpp>
 
-namespace Ogre
-{
-  class Vector3;
-  class Quaternion;
-  class ManualObject;
-  class ColourValue;
+namespace Ogre {
+class Vector3;
+class Quaternion;
+class ManualObject;
+class ColourValue;
 }
 
-namespace rviz
-{
-  class BillboardLine;
+namespace rviz {
+class BillboardLine;
 }
 
-namespace grid_map_rviz_plugin
+namespace grid_map_rviz_plugin {
+
+// Visualizes a single grid_map_msgs::GridMap message.
+class GridMapVisual
 {
-  // An instance of GridMapVisual visualizes a single grid_map_msgs::GridMap message
-  class GridMapVisual
-  {
-  public:
-    // Constructor
-    GridMapVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node);
-
-    // Destructor
-    virtual ~GridMapVisual();
-
-    // Copy the grid map data to map_
-    void setMessage(const grid_map_msgs::GridMap::ConstPtr& msg);
-    // Compute the visualization of map_
-    void computeVisualization(float alpha,
-			      bool show_grid_lines,
-			      bool flat_terrain,
-			      std::string height_layer,
-			      bool flat_color,
-			      Ogre::ColourValue mesh_color,
-			      std::string color_layer,
-			      bool use_rainbow,
-			      Ogre::ColourValue min_color,
-			      Ogre::ColourValue max_color,
-			      bool autocompute_intensity,
-			      float min_intensity,
-			      float max_intensity);
-
-    // Set the coordinate frame pose
-    void setFramePosition(const Ogre::Vector3& position);
-    void setFrameOrientation(const Ogre::Quaternion& orientation);
-
-    // get grid map layer names
-    std::vector<std::string> getLayerNames();
-
-  private:
-    Ogre::SceneNode* frame_node_;
-    Ogre::SceneManager* scene_manager_;
-
-    // ManualObject for mesh display
-    Ogre::ManualObject* manual_object_;
-    Ogre::MaterialPtr material_;
-    std::string material_name_;
-
-    // lines for mesh
-    boost::shared_ptr<rviz::BillboardLine> mesh_lines_;
-
-    // grid map
-    grid_map::GridMap map_;
-    bool have_map_;
-
-    // helper functions
-    void normalizeIntensity(float& intensity, float min_intensity, float max_intensity);
-    Ogre::ColourValue getRainbowColor(float intensity);
-    Ogre::ColourValue getInterpolatedColor(float intensity,
-					   Ogre::ColourValue min_color,
-					   Ogre::ColourValue max_color);
-  };
-
-} // end namespace grid_map_rviz_plugin
+ public:
+  GridMapVisual(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode);
+  virtual ~GridMapVisual();
+
+  // Copy the grid map data to map_.
+  void setMessage(const grid_map_msgs::GridMap::ConstPtr& msg);
+  // Compute the visualization of map_.
+  void computeVisualization(float alpha, bool showGridLines, bool flatTerrain,
+                            std::string heightLayer, bool flatColor, Ogre::ColourValue meshColor,
+                            std::string colorLayer, bool useRainbow, Ogre::ColourValue minColor,
+                            Ogre::ColourValue maxColor, bool autocomputeIntensity,
+                            float minIntensity, float maxIntensity);
+
+  // Set the coordinate frame pose.
+  void setFramePosition(const Ogre::Vector3& position);
+  void setFrameOrientation(const Ogre::Quaternion& orientation);
+
+  // Get grid map layer names.
+  std::vector<std::string> getLayerNames();
+
+ private:
+  Ogre::SceneNode* frameNode_;
+  Ogre::SceneManager* sceneManager_;
+
+  // ManualObject for mesh display.
+  Ogre::ManualObject* manualObject_;
+  Ogre::MaterialPtr material_;
+  std::string materialName_;
+
+  // Lines for mesh.
+  boost::shared_ptr<rviz::BillboardLine> meshLines_;
+
+  // Grid map.
+  grid_map::GridMap map_;
+  bool haveMap_;
+
+  // Helper methods.
+  void normalizeIntensity(float& intensity, float minIntensity, float maxIntensity);
+  Ogre::ColourValue getRainbowColor(float intensity);
+  Ogre::ColourValue getInterpolatedColor(float intensity, Ogre::ColourValue minColor,
+                                         Ogre::ColourValue maxColor);
+};
+
+}  // namespace
diff --git a/grid_map_rviz_plugin/package.xml b/grid_map_rviz_plugin/package.xml
@@ -5,15 +5,18 @@
   <maintainer email="kruesip@ethz.ch">Philipp Krüsi</maintainer>
   <license>BSD</license>
   <author>Philipp Krüsi</author>
+  <author>Péter Fankhauser</author>
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>qtbase5-dev</build_depend>
   <build_depend>rviz</build_depend>
   <build_depend>grid_map_ros</build_depend>
+  <build_depend>grid_map_msgs</build_depend>
   <run_depend>libqt5-core</run_depend>
   <run_depend>libqt5-gui</run_depend>
   <run_depend>libqt5-widgets</run_depend>
   <run_depend>rviz</run_depend>
   <run_depend>grid_map_ros</run_depend>
+  <run_depend>grid_map_msgs</run_depend>
   <export>
       <rviz plugin="${prefix}/plugin_description.xml"/>
   </export>