D435 and R200

CMakeLists.txt

@@ -1,38 +1,54 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(realsense_ros_gazebo)
 
-find_package(catkin REQUIRED COMPONENTS roscpp std_msgs std_srvs geometry_msgs nav_msgs tf dynamic_reconfigure message_generation)
-include_directories(include ${catkin_INCLUDE_DIRS})
-
-if(POLICY CMP0054)
-  cmake_policy(SET CMP0054 NEW)
-endif()
-
-find_package(gazebo REQUIRED)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
-include_directories(${GAZEBO_INCLUDE_DIRS})
-link_directories(${GAZEBO_LIBRARY_DIRS})
+find_package(catkin REQUIRED COMPONENTS
+  gazebo_dev
+  gazebo_msgs
+  roscpp
+  nodelet
+  sensor_msgs
+  urdf
+  tf
+  tf2_ros
+  rosconsole
+  diagnostic_updater
+  std_msgs
+  gazebo_ros
+  image_transport
+  camera_info_manager
+)
 
 find_package(Boost REQUIRED COMPONENTS thread)
-include_directories(${Boost_INCLUDE_DIRS})
-
-add_service_files(
-  FILES
-  SetBias.srv
-)
+# find_package(gazebo REQUIRED)
 
-generate_messages(
-  DEPENDENCIES geometry_msgs
+include_directories(include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
 )
 
-generate_dynamic_reconfigure_options(
-  cfg/SensorModel.cfg
+link_directories(
+  ${catkin_LIBRARY_DIRS}
 )
 
 catkin_package(
-    CATKIN_DEPENDS roscpp std_msgs geometry_msgs nav_msgs tf message_runtime
-    INCLUDE_DIRS include
-    LIBRARIES
+  INCLUDE_DIRS include
+  LIBRARIES
+  CATKIN_DEPENDS
+  message_runtime
+  gazebo_msgs
+  roscpp
+  nodelet
+  geometry_msgs
+  sensor_msgs
+  urdf
+  tf
+  tf2_ros
+  rosconsole
+  std_msgs
+  gazebo_ros
+  image_transport
+  camera_info_manager
 )
 
 ###########

README.md

@@ -1,7 +1,10 @@
-# realsense_ros_gazebo
-Intel Realsense Tracking and Depth camera simulations
+# Intel RealSense Gazebo/ROS
+Intel Realsense Tracking and Depth camera simulations and URDF macros.
+
+
+
+## RealSense T265 ##
 
-# Realsense T265 #
 ![](doc/t265.png)
 
 ### Usage ###
@@ -14,20 +17,15 @@ Intel Realsense Tracking and Depth camera simulations
 ```
 
 ### Publishers ###
-* /camera/**fisheye1**/camera_info
-* /camera/fisheye1/image_raw
-* /camera/fisheye1/image_raw/compressed
-* /camera/fisheye1/parameter_descriptions
-* /camera/fisheye1/parameter_updates
-* /camera/fisheye2/camera_info
-* /camera/fisheye2/image_raw
-* /camera/fisheye2/image_raw/compressed
-* /camera/fisheye2/parameter_descriptions
-* /camera/fisheye2/parameter_updates
+* /camera/**fisheye1**/*
+* /camera/**fisheye2**/*
 * /camera/**gyro**/sample _( accel and gyro are in the same imu message )_
 * /camera/**odom**/sample
 
-# Realsense R200 #
+
+
+## RealSense R200 ##
+
 ![](doc/r200.png)
 
 ### Usage ###
@@ -41,4 +39,30 @@ Intel Realsense Tracking and Depth camera simulations
 
 ### Publishers ###
 
-:construction:
+* /camera/**color**/*
+* /camera/**depth**/*
+* /camera/**infra1**/*
+* /camera/**infra2**/*
+
+
+
+## RealSense D435
+
+![](doc/d435.png)
+
+### Usage ###
+
+```xml
+<xacro:include filename="$(find realsense_ros_gazebo)/xacro/depthcam.xacro"/>
+
+<xacro:realsense_d435 sensor_name="d435" parent_link="base_link" rate="10">
+  <origin rpy="0 0 0 " xyz="0 0 0.5"/>
+</xacro:realsense_d435>
+```
+
+### Publishers ###
+
+* /camera/**color**/*
+* /camera/**depth**/*
+* /camera/**infra1**/*
+* /camera/**infra2**/*

include/realsense_gazebo_plugin/RealSensePlugin.h

@@ -25,92 +25,115 @@
 #include <gazebo/sensors/sensors.hh>
 #include <sdf/sdf.hh>
 
-#include <string>
 #include <memory>
+#include <string>
+
+namespace gazebo {
+#define DEPTH_CAMERA_NAME "depth"
+#define COLOR_CAMERA_NAME "color"
+#define IRED1_CAMERA_NAME "ired1"
+#define IRED2_CAMERA_NAME "ired2"
+
+struct CameraParams {
+  CameraParams() {}
+
+  std::string topic_name;
+  std::string camera_info_topic_name;
+  std::string optical_frame;
+};
+
+/// \brief A plugin that simulates Real Sense camera streams.
+class RealSensePlugin : public ModelPlugin {
+  /// \brief Constructor.
+public:
+  RealSensePlugin();
+
+  /// \brief Destructor.
+  ~RealSensePlugin();
+
+  // Documentation Inherited.
+  virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
 
-namespace gazebo
-{
+  /// \brief Callback for the World Update event.
+  void OnUpdate();
 
-  #define DEPTH_CAMERA_NAME "depth"
-  #define COLOR_CAMERA_NAME "color"
-  #define IRED1_CAMERA_NAME "ired1"
-  #define IRED2_CAMERA_NAME "ired2"
+  /// \brief Callback that publishes a received Depth Camera Frame as an
+  /// ImageStamped
+  /// message.
+  virtual void OnNewDepthFrame();
 
-  /// \brief A plugin that simulates Real Sense camera streams.
-  class RealSensePlugin : public ModelPlugin
-  {
-    /// \brief Constructor.
-    public: RealSensePlugin();
+  /// \brief Callback that publishes a received Camera Frame as an
+  /// ImageStamped message.
+  virtual void OnNewFrame(const rendering::CameraPtr cam,
+                          const transport::PublisherPtr pub);
 
-    /// \brief Destructor.
-    public: ~RealSensePlugin();
+protected:
+  /// \brief Pointer to the model containing the plugin.
+  physics::ModelPtr rsModel;
 
-    // Documentation Inherited.
-    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+  /// \brief Pointer to the world.
+  physics::WorldPtr world;
 
-    /// \brief Callback for the World Update event.
-    public: void OnUpdate();
+  /// \brief Pointer to the Depth Camera Renderer.
+  rendering::DepthCameraPtr depthCam;
 
-    /// \brief Callback that publishes a received Depth Camera Frame as an
-    /// ImageStamped
-    /// message.
-    public: virtual void OnNewDepthFrame();
+  /// \brief Pointer to the Color Camera Renderer.
+  rendering::CameraPtr colorCam;
 
-    /// \brief Callback that publishes a received Camera Frame as an
-    /// ImageStamped message.
-    public: virtual void OnNewFrame(const rendering::CameraPtr cam,
-                                    const transport::PublisherPtr pub);
+  /// \brief Pointer to the Infrared Camera Renderer.
+  rendering::CameraPtr ired1Cam;
 
-    /// \brief Pointer to the model containing the plugin.
-    protected: physics::ModelPtr rsModel;
+  /// \brief Pointer to the Infrared2 Camera Renderer.
+  rendering::CameraPtr ired2Cam;
 
-    /// \brief Pointer to the world.
-    protected: physics::WorldPtr world;
+  /// \brief String to hold the camera prefix
+  std::string prefix;
 
-    /// \brief Pointer to the Depth Camera Renderer.
-    protected: rendering::DepthCameraPtr depthCam;
+  /// \brief Pointer to the transport Node.
+  transport::NodePtr transportNode;
 
-    /// \brief Pointer to the Color Camera Renderer.
-    protected: rendering::CameraPtr colorCam;
+  // \brief Store Real Sense depth map data.
+  std::vector<uint16_t> depthMap;
 
-    /// \brief Pointer to the Infrared Camera Renderer.
-    protected: rendering::CameraPtr ired1Cam;
+  /// \brief Pointer to the Depth Publisher.
+  transport::PublisherPtr depthPub;
 
-    /// \brief Pointer to the Infrared2 Camera Renderer.
-    protected: rendering::CameraPtr ired2Cam;
+  /// \brief Pointer to the Color Publisher.
+  transport::PublisherPtr colorPub;
 
-    /// \brief Pointer to the transport Node.
-    protected: transport::NodePtr transportNode;
+  /// \brief Pointer to the Infrared Publisher.
+  transport::PublisherPtr ired1Pub;
 
-    // \brief Store Real Sense depth map data.
-    protected: std::vector<uint16_t> depthMap;
+  /// \brief Pointer to the Infrared2 Publisher.
+  transport::PublisherPtr ired2Pub;
 
-    /// \brief Pointer to the Depth Publisher.
-    protected: transport::PublisherPtr depthPub;
+  /// \brief Pointer to the Depth Camera callback connection.
+  event::ConnectionPtr newDepthFrameConn;
 
-    /// \brief Pointer to the Color Publisher.
-    protected: transport::PublisherPtr colorPub;
+  /// \brief Pointer to the Depth Camera callback connection.
+  event::ConnectionPtr newIred1FrameConn;
 
-    /// \brief Pointer to the Infrared Publisher.
-    protected: transport::PublisherPtr ired1Pub;
+  /// \brief Pointer to the Infrared Camera callback connection.
+  event::ConnectionPtr newIred2FrameConn;
 
-    /// \brief Pointer to the Infrared2 Publisher.
-    protected: transport::PublisherPtr ired2Pub;
+  /// \brief Pointer to the Color Camera callback connection.
+  event::ConnectionPtr newColorFrameConn;
 
-    /// \brief Pointer to the Depth Camera callback connection.
-    protected: event::ConnectionPtr newDepthFrameConn;
+  /// \brief Pointer to the World Update event connection.
+  event::ConnectionPtr updateConnection;
 
-    /// \brief Pointer to the Depth Camera callback connection.
-    protected: event::ConnectionPtr newIred1FrameConn;
+  std::map<std::string, CameraParams> cameraParamsMap_;
 
-    /// \brief Pointer to the Infrared Camera callback connection.
-    protected: event::ConnectionPtr newIred2FrameConn;
+  bool pointCloud_ = false;
+  std::string pointCloudTopic_;
+  double pointCloudCutOff_, pointCloudCutOffMax_;
 
-    /// \brief Pointer to the Color Camera callback connection.
-    protected: event::ConnectionPtr newColorFrameConn;
+  double colorUpdateRate_;
+  double infraredUpdateRate_;
+  double depthUpdateRate_;
 
-    /// \brief Pointer to the World Update event connection.
-    protected: event::ConnectionPtr updateConnection;
-  };
+  float rangeMinDepth_;
+  float rangeMaxDepth_;
+};
 }
 #endif