T265

CMakeLists.txt

@@ -0,0 +1,69 @@
+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(Boost REQUIRED COMPONENTS thread)
+include_directories(${Boost_INCLUDE_DIRS})
+
+add_service_files(
+  FILES
+  SetBias.srv
+)
+
+generate_messages(
+  DEPENDENCIES geometry_msgs
+)
+
+generate_dynamic_reconfigure_options(
+  cfg/SensorModel.cfg
+)
+
+catkin_package(
+    CATKIN_DEPENDS roscpp std_msgs geometry_msgs nav_msgs tf message_runtime
+    INCLUDE_DIRS include
+    LIBRARIES
+)
+
+###########
+## Build ##
+###########
+
+add_library(realsense_gazebo_plugin src/RealSensePlugin.cpp src/gazebo_ros_realsense.cpp)
+target_link_libraries(realsense_gazebo_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+add_dependencies(realsense_gazebo_plugin ${catkin_EXPORTED_TARGETS})
+
+#############
+## Install ##
+#############
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+install(TARGETS
+  realsense_gazebo_plugin
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY meshes
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY xacro
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

README.md

@@ -1,2 +1,44 @@
 # realsense_ros_gazebo
 Intel Realsense Tracking and Depth camera simulations
+
+# Realsense T265 #
+![](doc/t265.png)
+
+### Usage ###
+```xml
+<xacro:include filename="$(find realsense_ros_gazebo)/xacro/tracker.xacro"/>
+
+<xacro:realsense_T265 sensor_name="camera" parent_link="base_link" rate="30.0">
+  <origin rpy="0 0 0" xyz="0 0 0.5"/>
+</xacro:realsense_T265>
+```
+
+### 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/**gyro**/sample _( accel and gyro are in the same imu message )_
+* /camera/**odom**/sample
+
+# Realsense R200 #
+![](doc/r200.png)
+
+### Usage ###
+```xml
+<xacro:include filename="$(find realsense_ros_gazebo)/xacro/depthcam.xacro"/>
+
+<xacro:realsense_R200 sensor_name="camera" parent_link="base_link" rate="30.0">
+  <origin rpy="0 0 0" xyz="0 0 0.5"/>
+</xacro:realsense_R200>
+```
+
+### Publishers ###
+
+:construction:

cfg/SensorModel.cfg

@@ -0,0 +1,14 @@
+#!/usr/bin/env python
+PACKAGE = "nowtech_sensor_models"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("gaussian_noise",      double_t, 1, "Standard deviation of the additive white Gaussian noise", 0.0, 0.0, 10.0)
+gen.add("offset",              double_t, 1, "Zero-offset of the published sensor signal", 0.0, -10.0, 10.0)
+gen.add("drift",               double_t, 1, "Standard deviation of the sensor drift", 0.0, 0.0, 10.0)
+gen.add("drift_frequency",     double_t, 1, "Reciprocal of the time constant of the first-order drift model in Hz", 0.0, 0.0, 1.0)
+gen.add("scale_error",         double_t, 1, "Scale error", 1.0, 0.0, 2.0)
+
+exit(gen.generate(PACKAGE, "nowtech_sensor_models", "SensorModel"))

include/realsense_gazebo_plugin/RealSensePlugin.h

@@ -0,0 +1,116 @@
+/*
+// Copyright (c) 2016 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+*/
+
+#ifndef _GZRS_PLUGIN_HH_
+#define _GZRS_PLUGIN_HH_
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/physics/PhysicsTypes.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/rendering/DepthCamera.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <sdf/sdf.hh>
+
+#include <string>
+#include <memory>
+
+namespace gazebo
+{
+
+  #define DEPTH_CAMERA_NAME "depth"
+  #define COLOR_CAMERA_NAME "color"
+  #define IRED1_CAMERA_NAME "ired1"
+  #define IRED2_CAMERA_NAME "ired2"
+
+  /// \brief A plugin that simulates Real Sense camera streams.
+  class RealSensePlugin : public ModelPlugin
+  {
+    /// \brief Constructor.
+    public: RealSensePlugin();
+
+    /// \brief Destructor.
+    public: ~RealSensePlugin();
+
+    // Documentation Inherited.
+    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+    /// \brief Callback for the World Update event.
+    public: void OnUpdate();
+
+    /// \brief Callback that publishes a received Depth Camera Frame as an
+    /// ImageStamped
+    /// message.
+    public: virtual void OnNewDepthFrame();
+
+    /// \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 model containing the plugin.
+    protected: physics::ModelPtr rsModel;
+
+    /// \brief Pointer to the world.
+    protected: physics::WorldPtr world;
+
+    /// \brief Pointer to the Depth Camera Renderer.
+    protected: rendering::DepthCameraPtr depthCam;
+
+    /// \brief Pointer to the Color Camera Renderer.
+    protected: rendering::CameraPtr colorCam;
+
+    /// \brief Pointer to the Infrared Camera Renderer.
+    protected: rendering::CameraPtr ired1Cam;
+
+    /// \brief Pointer to the Infrared2 Camera Renderer.
+    protected: rendering::CameraPtr ired2Cam;
+
+    /// \brief Pointer to the transport Node.
+    protected: transport::NodePtr transportNode;
+
+    // \brief Store Real Sense depth map data.
+    protected: std::vector<uint16_t> depthMap;
+
+    /// \brief Pointer to the Depth Publisher.
+    protected: transport::PublisherPtr depthPub;
+
+    /// \brief Pointer to the Color Publisher.
+    protected: transport::PublisherPtr colorPub;
+
+    /// \brief Pointer to the Infrared Publisher.
+    protected: transport::PublisherPtr ired1Pub;
+
+    /// \brief Pointer to the Infrared2 Publisher.
+    protected: transport::PublisherPtr ired2Pub;
+
+    /// \brief Pointer to the Depth Camera callback connection.
+    protected: event::ConnectionPtr newDepthFrameConn;
+
+    /// \brief Pointer to the Depth Camera callback connection.
+    protected: event::ConnectionPtr newIred1FrameConn;
+
+    /// \brief Pointer to the Infrared Camera callback connection.
+    protected: event::ConnectionPtr newIred2FrameConn;
+
+    /// \brief Pointer to the Color Camera callback connection.
+    protected: event::ConnectionPtr newColorFrameConn;
+
+    /// \brief Pointer to the World Update event connection.
+    protected: event::ConnectionPtr updateConnection;
+  };
+}
+#endif

include/realsense_gazebo_plugin/gazebo_ros_realsense.h

@@ -0,0 +1,52 @@
+#ifndef _GAZEBO_ROS_REALSENSE_PLUGIN_
+#define _GAZEBO_ROS_REALSENSE_PLUGIN_
+
+#include "realsense_gazebo_plugin/RealSensePlugin.h"
+
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include <image_transport/image_transport.h>
+#include <camera_info_manager/camera_info_manager.h>
+
+#include <string>
+#include <memory>
+
+namespace gazebo
+{
+  /// \brief A plugin that simulates Real Sense camera streams.
+  class GazeboRosRealsense : public RealSensePlugin
+  {
+    /// \brief Constructor.
+    public: GazeboRosRealsense();
+
+    /// \brief Destructor.
+    public: ~GazeboRosRealsense();
+
+    // Documentation Inherited.
+    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+    /// \brief Callback that publishes a received Depth Camera Frame as an
+    /// ImageStamped message.
+    public: virtual void OnNewDepthFrame();
+
+    /// \brief Callback that publishes a received Camera Frame as an
+    /// ImageStamped message.
+    public: virtual void OnNewFrame(const rendering::CameraPtr cam,
+                                    const transport::PublisherPtr pub);
+
+    protected: boost::shared_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
+
+    /// \brief A pointer to the ROS node.
+    ///  A node will be instantiated if it does not exist.
+    protected: ros::NodeHandle* rosnode_;
+    private: image_transport::ImageTransport* itnode_;
+    protected: image_transport::CameraPublisher color_pub_, ir1_pub_, ir2_pub_, depth_pub_;
+
+    /// \brief ROS image messages
+    protected: sensor_msgs::Image image_msg_, depth_msg_;
+  };
+}
+#endif /* _GAZEBO_ROS_REALSENSE_PLUGIN_ */

launch/simulation.launch

@@ -0,0 +1,24 @@
+<launch>
+
+  <!-- Model setup -->
+  <arg name="model" default="$(find realsense_ros_gazebo)/urdf/test.xacro"/>
+
+  <!-- spawn urdf model to gazebo -->
+  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
+  <node  name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" args="-z 1.0 -unpause -urdf -model test_model -param robot_description" respawn="false" output="screen" />
+
+  <!-- simulator -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="paused" value="true"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/>
+    <arg name="recording" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+  <!-- state publishers -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+</launch>