WIP: Plugin-based simulator

CMakeLists.txt

@@ -8,6 +8,8 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_ros # Used for interfaces extension
   tf2_ros # Used for ROS example
   tf2_eigen # Used for ROS example
+  pluginlib
+  urdf
   visualization_msgs # Used for ROS example
 )
 
@@ -28,6 +30,8 @@ catkin_package(
     pcl_ros
     tf2_ros
     tf2_eigen
+    pluginlib
+    urdf
     visualization_msgs
   DEPENDS OpenCV
 )
@@ -115,6 +119,26 @@ target_link_libraries(${PROJECT_NAME}_laser_example
   ${catkin_LIBRARIES}
 )
 
+add_library(${PROJECT_NAME}_simulator_plugins
+  src/simulator/ros_urdf_scene_updater.cpp
+  src/simulator/depth_camera_plugin.cpp
+  src/simulator/laser_scanner_plugin.cpp
+)
+target_link_libraries(${PROJECT_NAME}_simulator_plugins
+  ${catkin_LIBRARIES}
+  ${PROJECT_NAME}
+  ${PROJECT_NAME}_interfaces
+  ${PROJECT_NAME}_sim_laser_scanner
+)
+
+add_executable(${PROJECT_NAME}_simulator
+  src/simulator/simulator.cpp
+)
+target_link_libraries(${PROJECT_NAME}_simulator
+  ${catkin_LIBRARIES}
+  ${PROJECT_NAME}
+)
+
 install(
   TARGETS
     ${PROJECT_NAME}
@@ -124,6 +148,8 @@ install(
     ${PROJECT_NAME}_orbit
     ${PROJECT_NAME}_ros_orbit
     ${PROJECT_NAME}_laser_example
+    ${PROJECT_NAME}_simulator_plugins
+    ${PROJECT_NAME}_simulator
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -133,6 +159,10 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
 
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
 #############
 ## Testing ##
 #############

include/gl_depth_sim/sim_depth_camera.h

@@ -21,7 +21,7 @@ namespace gl_depth_sim
  */
 struct RenderableObjectState
 {
-  std::unique_ptr<RenderableMesh> mesh;
+  std::shared_ptr<RenderableMesh> mesh;
   Eigen::Isometry3d pose;
 };
 
@@ -53,6 +53,7 @@ class SimDepthCamera
    * @return A data structure that contains a linearized depth data matrix. See @class DepthImage.
    */
   DepthImage render(const Eigen::Isometry3d& pose);
+  DepthImage render(const Eigen::Isometry3d& pose, const std::map<std::string, RenderableObjectState>& objects);
 
   /**
    * @brief Adds a triangle mesh to the scene with the given pose in world coordinates.

include/gl_depth_sim/sim_laser_scanner.h

@@ -58,7 +58,8 @@ class SimLaserScanner
    * @param scanner_pose
    * @return
    */
-  const pcl::PointCloud<pcl::PointXYZ> render(const Eigen::Isometry3d& scanner_pose);
+  pcl::PointCloud<pcl::PointXYZ> render(const Eigen::Isometry3d& scanner_pose);
+  pcl::PointCloud<pcl::PointXYZ> render(const Eigen::Isometry3d& scanner_pose, const std::map<std::string, RenderableObjectState>& scene);
 
   /**
    * @brief Adds a mesh to the renderable environment

include/gl_depth_sim/simulator/depth_camera_plugin.h

@@ -0,0 +1,35 @@
+#ifndef GL_DEPTH_SIM_SIMULATOR_DEPTH_CAMERA_PLUGIN_H
+#define GL_DEPTH_SIM_SIMULATOR_DEPTH_CAMERA_PLUGIN_H
+
+#include <gl_depth_sim/simulator/simulator_plugins.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace gl_depth_sim
+{
+/**
+ * @brief Depth camera implementation of the render plugin
+ */
+class DepthCameraPlugin : public RenderPlugin
+{
+public:
+  DepthCameraPlugin();
+
+  virtual void init(const XmlRpc::XmlRpcValue &config) override;
+  virtual void render(const std::map<std::string, RenderableObjectState>& scene) override;
+
+private:
+  tf2_ros::Buffer buffer_;
+  tf2_ros::TransformListener listener_;
+
+  std::string fixed_frame_;
+  std::string camera_frame_;
+
+  gl_depth_sim::CameraProperties props_;
+  std::unique_ptr<gl_depth_sim::SimDepthCamera> sim_;
+
+  ros::Publisher pub_;
+};
+
+} // namespace gl_depth_sim
+
+#endif // GL_DEPTH_SIM_SIMULATOR_DEPTH_CAMERA_PLUGIN_H

include/gl_depth_sim/simulator/laser_scanner_plugin.h

@@ -0,0 +1,36 @@
+#ifndef GL_DEPTH_SIM_SIMULATOR_LASER_SCANNER_PLUGIN_H
+#define GL_DEPTH_SIM_SIMULATOR_LASER_SCANNER_PLUGIN_H
+
+#include <gl_depth_sim/simulator/simulator_plugins.h>
+#include <gl_depth_sim/sim_laser_scanner.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace gl_depth_sim
+{
+/**
+ * @brief Laser scanner implementation of the render plugin
+ */
+class LaserScannerPlugin : public RenderPlugin
+{
+public:
+  LaserScannerPlugin();
+
+  virtual void init(const XmlRpc::XmlRpcValue& config) override;
+  virtual void render(const std::map<std::string, RenderableObjectState>& scene) override;
+
+private:
+  tf2_ros::Buffer buffer_;
+  tf2_ros::TransformListener listener_;
+
+  std::string fixed_frame_;
+  std::string scanner_frame_;
+
+  gl_depth_sim::LaserScannerProperties props_;
+  std::unique_ptr<gl_depth_sim::SimLaserScanner> sim_;
+
+  ros::Publisher pub_;
+};
+
+} // namespace gl_depth_sim
+
+#endif // GL_DEPTH_SIM_SIMULATOR_LASER_SCANNER_PLUGIN_H

include/gl_depth_sim/simulator/ros_urdf_scene_updater.h

@@ -0,0 +1,36 @@
+#ifndef GL_DEPTH_SIM_SIMULATOR_ROS_SCENE_UPDATER_H
+#define GL_DEPTH_SIM_SIMULATOR_ROS_SCENE_UPDATER_H
+
+#include <gl_depth_sim/simulator/simulator_plugins.h>
+#include <tf2_ros/transform_listener.h>
+#include <ros/ros.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace gl_depth_sim
+{
+/**
+ * @brief Scene updater plugin which creates a scene from URDF and updates it using transform information from TF
+ */
+class ROSURDFSceneUpdaterPlugin : public SceneUpdaterPlugin
+{
+public:
+  ROSURDFSceneUpdaterPlugin();
+
+  virtual void init(const XmlRpc::XmlRpcValue &config) override;
+
+  virtual void createScene() override;
+  virtual void updateScene() override;
+
+private:
+  std::string fixed_frame_;
+
+  tf2_ros::Buffer buffer_;
+  tf2_ros::TransformListener listener_;
+
+  std::map<std::string, Eigen::Isometry3d> relative_poses_;
+};
+
+} // namespace gl_depth_sim
+
+#endif // GL_DEPTH_SIM_SIMULATOR_ROS_SCENE_UPDATER_H

include/gl_depth_sim/simulator/simulator_plugins.h

@@ -0,0 +1,75 @@
+#ifndef GL_DEPTH_SIM_SIMULATOR_SIMULATOR_PLUGINS_H
+#define GL_DEPTH_SIM_SIMULATOR_SIMULATOR_PLUGINS_H
+
+#include <gl_depth_sim/sim_depth_camera.h>
+
+#include <xmlrpcpp/XmlRpcValue.h>
+#include <boost/shared_ptr.hpp>
+
+namespace gl_depth_sim
+{
+/**
+ * @brief Base class plugin to create and update a renderable scene
+ */
+class SceneUpdaterPlugin
+{
+public:
+  using Ptr = boost::shared_ptr<SceneUpdaterPlugin>;
+  SceneUpdaterPlugin() = default;
+  virtual ~SceneUpdaterPlugin() = default;
+
+  /**
+   * @brief Initializes the plugin from an XMLRPC configuration
+   * @param config
+   */
+  virtual void init(const XmlRpc::XmlRpcValue& config) = 0;
+
+  /**
+   * @brief Creates a scene with renderable objects
+   */
+  virtual void createScene() = 0;
+
+  /**
+   * @brief Updates the location of renderable objects within the scene
+   */
+  virtual void updateScene() = 0;
+
+  /**
+   * @brief Returns the representation of the scene
+   * @return
+   */
+  inline const std::map<std::string, RenderableObjectState>& getScene() const
+  {
+    return scene_;
+  }
+
+protected:
+  std::map<std::string, RenderableObjectState> scene_;
+};
+
+/**
+ * @brief Base class plugin for rendering a scene
+ */
+class RenderPlugin
+{
+public:
+  using Ptr = boost::shared_ptr<RenderPlugin>;
+  RenderPlugin() = default;
+  virtual ~RenderPlugin() = default;
+
+  /**
+   * @brief Initializes the plugin with an XMLRPC configuration
+   * @param config
+   */
+  virtual void init(const XmlRpc::XmlRpcValue& config) = 0;
+
+  /**
+   * @brief Renders the input scene
+   * @param scene
+   */
+  virtual void render(const std::map<std::string, RenderableObjectState>& scene) = 0;
+};
+
+}  // namespace gl_depth_sim
+
+#endif  // GL_DEPTH_SIM_SIMULATOR_SIMULATOR_PLUGINS_H

package.xml

@@ -15,11 +15,17 @@
   <depend>tf2_ros</depend>
   <depend>tf2_eigen</depend>
   <depend>pcl_ros</depend>
+  <depend>pluginlib</depend>
+  <depend>urdf</depend>
   <depend>visualization_msgs</depend>
 
   <!-- system dependency -->
   <build_depend>libglfw3-dev</build_depend>
   <depend>assimp</depend>
   <depend>libopencv-dev</depend>
 
+  <export>
+    <gl_depth_sim plugin="${prefix}/plugin_description.xml"/>
+  </export>
+
 </package>

plugin_description.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<library path="lib/libgl_depth_sim_simulator_plugins">
+  <class name="gl_depth_sim/ROSURDFSceneUpdaterPlugin"
+         type="gl_depth_sim::ROSURDFSceneUpdaterPlugin"
+         base_class_type="gl_depth_sim::SceneUpdaterPlugin">
+    <description>
+      Scene updater plugin that creates a scene from a URDF and updates it with transform information from TF
+    </description>
+  </class>
+  <class name="gl_depth_sim/DepthCameraPlugin"
+         type="gl_depth_sim::DepthCameraPlugin"
+         base_class_type="gl_depth_sim::RenderPlugin">
+    <description>
+      Render plugin for a 3D depth camera
+    </description>
+  </class>
+  <class name="gl_depth_sim/LaserScannerPlugin"
+         type="gl_depth_sim::LaserScannerPlugin"
+         base_class_type="gl_depth_sim::RenderPlugin">
+    <description>
+      Render plugin for a 2D laser scanner
+    </description>
+  </class>
+</library>