Merge pull request avidbots#51 from avidbots/model-preview

Model preview in flatland_viz (fixes avidbots#15)

flatland_server/launch/server.launch

@@ -27,7 +27,7 @@
   </node>
 
 <group if="$(arg show_viz)">
-  <node name="flatland_viz" pkg="flatland_viz" type="flatland_viz" required="true" unless="$(arg use_rviz)"/>
+  <node name="flatland_viz" pkg="flatland_viz" type="flatland_viz" output="screen" required="true" unless="$(arg use_rviz)"/>
 </group>
 
 

flatland_server/test/conestogo_office_test/cleaner.model.yaml

@@ -12,65 +12,6 @@ bodies:
                   [.30, .16111],
                   [.07983, 0.337],
                   [-1.03, 0.337] ]
-
-      #   points: [
-
-      #     [0, -1.03],
-      #     [-0.337, -1.03],
-      #     [-0.337, 0.10983],
-      #     [-0.31971, 0.15634],
-      #     [-0.27569, 0.21002],
-      #     [-0.18131, 0.2756],
-      #     [-0.11, 0.30058],
-      #     [0, 0.30058]
-
-
-      #   ]
-
-      # - type: polygon
-      #   density: 1000000.0
-      #   sensor: true
-      #   points: [
-
-      #    [0, -1.03],
-      #     [0, 0.30058],
-      #     [0.11, 0.30058],
-      #     [0.18131, 0.2756],
-      #     [0.27569, 0.21002],
-      #     [0.31971, 0.15634],
-      #     [0.337, 0.10983],
-      #     [0.337, -1.03]
-
-      #   ]
-
-      #   points: [
-
-      #   [-0, 0], 
-      #   [-0.337, 0], 
-      #   [-0.337, 1.13983], 
-      #   [-0.31971, 1.18634], 
-      #   [-0.27569, 1.24002], 
-      #   [-0.18131, 1.3056], 
-      #   [-0.11, 1.33058],
-      #   [-0,  1.33058]
-
-      #   ]
-
-      # - type: polygon
-      #   sensor: true
-      #   points: [
-
-      #   [0, 0], 
-      #   [0,  1.33058],
-      #   [0.11, 1.33058],
-      #   [0.18131, 1.3056], 
-      #   [0.27569, 1.24002], 
-      #   [0.31971, 1.18634], 
-      #   [0.337, 1.13983], 
-      #   [0.337, 0]
-
-      #   ]
-
   - name: front_wheel
     color: [1, 1, 1, 0.75] 
     footprints:
@@ -80,7 +21,6 @@ bodies:
                  [ 0.0875,  0.0250],
                  [-0.0875, 0.0250],
                  [-0.0875, -0.0250]]
-        # points: [[-.3155, -.7425], [-.2645, -.7425], [-.2645, -.9175], [-.3155, -.9175]]
 
   - name: rear_left_wheel
     color: [1, 1, 1, 0.75] 
@@ -93,7 +33,7 @@ bodies:
                  [-0.0875, -0.0255]]
 
   - name: rear_right_wheel
-    color: [1, 1, 1, 0.75] 
+    color: [1, 1, 1, 0.75]
     footprints:
       - type: polygon
         density: 1.0

flatland_viz/CMakeLists.txt

@@ -134,9 +134,6 @@ install(TARGETS flatland_viz flatland_viz_plugins
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-install(DIRECTORY media/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media)
-
 install(FILES
    plugin_description.xml
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

flatland_viz/include/flatland_viz/spawn_model_tool.h

@@ -49,13 +49,17 @@
 #define SPAWN_MODEL_TOOL_H
 
 #include <rviz/tool.h>
+#include <memory>
+#include <vector>
 
 #include <OGRE/OgreEntity.h>
 #include <OGRE/OgreSceneManager.h>
 #include <OGRE/OgreSceneNode.h>
 #include <OgreVector3.h>
 
+#include <flatland_server/yaml_reader.h>
 #include <ros/ros.h>
+#include <rviz/ogre_helpers/billboard_line.h>
 #include "rviz/ogre_helpers/arrow.h"
 
 namespace flatland_viz {
@@ -122,23 +126,44 @@ class SpawnModelTool : public rviz::Tool {
   * @param c             QColor to set the 3d model
   */
   void SetMovingModelColor(QColor c);
+  /**
+   * @name               LoadPreview
+   * @brief              Load a vector preview of the model
+   */
+  void LoadPreview();
+  /**
+   * @name               LoadPolygonFootprint
+   * @brief              Load a vector preview of the model's polygon footprint
+   * @param footprint    The footprint yaml node
+   * @param pose         x,y,theta pose of footprint
+   */
+  void LoadPolygonFootprint(flatland_server::YamlReader &footprint,
+                            const flatland_server::Pose pose);
+  /**
+   * @name               LoadCircleFootprint
+   * @brief              Load a vector preview of the model's circle footprint
+   * @param footprint    The footprint yaml node
+   * @param pose         x,y,theta pose of footprint
+   */
+  void LoadCircleFootprint(flatland_server::YamlReader &footprint,
+                           const flatland_server::Pose pose);
 
   Ogre::Vector3
       intersection;     // location cursor intersects ground plane, ie the
                         // location to create the model
   float initial_angle;  // the angle to create the model at
   Ogre::SceneNode *moving_model_node_;  // the node for the 3D object
-  std::string model_resource_;          // path to 3d model
   enum ModelState { m_hidden, m_dragging, m_rotating };
   ModelState model_state;  // model state, first hidden, then dragging to
                            // intersection point, then rotating to desired angle
-  static QString path_to_model_file;  // full path to model file (yaml)
+  static QString path_to_model_file_;  // full path to model file (yaml)
   static QString model_name;  // base file name with path and extension removed
 
  protected:
   rviz::Arrow *arrow_;        // Rviz 3d arrow to show axis of rotation
   ros::NodeHandle nh;         // ros service node handle
   ros::ServiceClient client;  // ros service client
+  std::vector<std::shared_ptr<rviz::BillboardLine>> lines_list_;
 };
 
 }  // end namespace flatland_viz