added gazebo plugins for d435i's IMU

CMakeLists.txt

@@ -40,9 +40,18 @@ add_library(
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
 add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
 
+add_library(realsense_gazebo_accel_sensor src/gazebo_ros_accel_sensor.cpp)
+target_link_libraries(realsense_gazebo_accel_sensor ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+add_library(realsense_gazebo_gyro_sensor src/gazebo_ros_gyro_sensor.cpp)
+target_link_libraries(realsense_gazebo_gyro_sensor ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+#add_dependencies(realsense_gazebo_gyro_sensor ${catkin_EXPORTED_TARGETS})
+
 install(
     TARGETS
         ${PROJECT_NAME}
+        realsense_gazebo_accel_sensor
+        realsense_gazebo_gyro_sensor
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )

README.md

@@ -1,7 +1,151 @@
 # Intel RealSense Gazebo ROS plugin
 
 This package is a Gazebo ROS plugin for the Intel D435 realsense camera.
-
+
+This fork adds gazebo plugins for the IMU found in the Intel D435i realsense camera.
+
+## Usage
+
+Download this package and put it into your catkinws src folder and then compile the catkin package.
+This will generate shared libraries called librealsense_gazebo_plugin.so,  for you which is used in the files you gonna download in the next steps.
+
+Download the two xacro files [```_d435.gazebo.xacro```](https://raw.githubusercontent.com/pal-robotics-forks/realsense/upstream/realsense2_description/urdf/_d435.gazebo.xacro) and [```_d435.urdf.xacro```](https://raw.githubusercontent.com/pal-robotics-forks/realsense/upstream/realsense2_description/urdf/_d435.urdf.xacro) (they are from [pal-robotics-forks/realsense](https://github.com/pal-robotics-forks/realsense/tree/upstream/realsense2_description/urdf)).
+Put those two files in the urdf folder of the package where you want your realsense to be simulated.
+
+**In the file ```_d435.urdf.xacro``` change:**
+
+**line 13** from:
+
+```xml
+<xacro:include filename="$(find realsense2_description)/urdf/urdf_d435.gazebo.xacro"/>
+```
+
+to: (note: replace ```packagename``` the name of your package)
+
+```xml
+<xacro:include filename="$(find packagename)/urdf/urdf_d435.gazebo.xacro"/>
+```
+
+**lines 144-145** from:
+
+```xml
+<!-- Realsense Gazebo Plugin -->
+<xacro:gazebo_d435 camera_name="${name}" reference_link="${name}_link" topics_ns="${topics_ns}" depth_optical_frame="${name}_depth_optical_frame" color_optical_frame="${name}_color_optical_frame" infrared1_optical_frame="${name}_left_ir_optical_frame" infrared2_optical_frame="${name}_right_ir_optical_frame"/>
+```
+
+to:
+
+```xml
+<!-- camera accel joints and links -->
+    <joint name="${name}_accel_joint" type="fixed">
+      <origin xyz="-0.00174 0.00552 0.0176" rpy="0 0 0" />
+      <parent link="${name}_link" />
+      <child link="${name}_accel_frame" />
+    </joint>
+    <link name="${name}_accel_frame"/>
+
+    <joint name="${name}_accel_optical_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
+      <parent link="${name}_accel_frame" />
+      <child link="${name}_accel_optical_frame" />
+    </joint>
+    <link name="${name}_accel_optical_frame"/>
+
+    <!-- camera gyro joints and links -->
+    <joint name="${name}_gyro_joint" type="fixed">
+      <origin xyz="-0.00174 0.00552 0.0176" rpy="0 0 0" />
+      <parent link="${name}_link" />
+      <child link="${name}_gyro_frame" />
+    </joint>
+    <link name="${name}_gyro_frame"/>
+
+    <joint name="${name}_gyro_optical_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
+      <parent link="${name}_gyro_frame" />
+      <child link="${name}_gyro_optical_frame" />
+    </joint>
+    <link name="${name}_gyro_optical_frame"/>
+
+    <!-- Realsense Gazebo Plugin -->
+    <xacro:gazebo_d435i camera_name="${name}"
+                        reference_link="${name}_link"
+                        topics_ns="${topics_ns}"
+                        depth_optical_frame="${name}_depth_optical_frame"
+                        color_optical_frame="${name}_color_optical_frame"
+                        infrared1_optical_frame="${name}_left_ir_optical_frame"
+                        infrared2_optical_frame="${name}_right_ir_optical_frame"
+                        accel_optical_frame="${name}_accel_optical_frame"
+                        gyro_optical_frame="${name}_gyro_optical_frame"
+    />
+```
+
+**In the file ```_d435.urdf.xacro``` change:**
+
+**line 12** from:
+
+```xml
+<xacro:macro name="gazebo_d435" params="camera_name reference_link topics_ns depth_optical_frame color_optical_frame infrared1_optical_frame infrared2_optical_frame" >
+```
+
+to:
+
+```xml
+<xacro:macro name="gazebo_d435i" params="camera_name reference_link topics_ns depth_optical_frame color_optical_frame infrared1_optical_frame infrared2_optical_frame accel_optical_frame gyro_optical_frame" >
+```
+
+**line 116-117** from:
+
+```xml
+  </sensor>
+</gazebo>
+```
+
+to:
+
+```xml
+  </sensor>
+  <sensor name="{camera_name}accel" type="imu">
+    <always_on>true</always_on>
+    <update_rate>300</update_rate>
+    <topic>${camera_name}/accel/sample</topic>
+    <plugin name="${topics_ns}_accel_plugin" filename="librealsense_gazebo_accel_sensor.so">
+      <topicName>${camera_name}/accel/sample</topicName>
+      <bodyName>imu_link</bodyName>
+      <updateRateHZ>300.0</updateRateHZ>
+      <gaussianNoise>0.1</gaussianNoise>
+      <xyzOffset>0 0 0</xyzOffset>
+      <rpyOffset>0 0 0</rpyOffset>
+      <frameName>${accel_optical_frame}</frameName>
+    </plugin>
+    <pose>0 0 0 0 0 0</pose>
+  </sensor>
+  <sensor name="{camera_name}gyro" type="imu">
+    <always_on>true</always_on>
+    <update_rate>300</update_rate>
+    <topic>${camera_name}/gyro/sample</topic>
+    <plugin name="${topics_ns}_gyro_plugin" filename="librealsense_gazebo_gyro_sensor.so">
+      <topicName>${camera_name}/gyro/sample</topicName>
+      <bodyName>imu_link</bodyName>
+      <updateRateHZ>300.0</updateRateHZ>
+      <gaussianNoise>0.1</gaussianNoise>
+      <xyzOffset>0 0 0</xyzOffset>
+      <rpyOffset>0 0 0</rpyOffset>
+      <frameName>${gyro_optical_frame}</frameName>
+    </plugin>
+    <pose>0 0 0 0 0 0</pose>
+  </sensor>
+</gazebo>
+```
+
+### Finally
+
+In the urdf file where you want to use the simulated realsense add the following code, while again replacing ```packagename```,```tf_prefix```, and ```topic_prefix``` with you are using.
+
+```xml
+<xacro:include filename="$(find packagename)/urdf/_d435.urdf.xacro" />
+<xacro:sensor_d435 parent="parent_link" name="tf_prefix" topics_ns="topic_prefix"/>
+```
+
 ## Acknowledgement
 
 This is a continuation of work done by [SyrianSpock](https://github.com/SyrianSpock) for a Gazebo ROS plugin with RS200 camera.

include/realsense_gazebo_plugin/gazebo_ros_accel_sensor.h

@@ -0,0 +1,107 @@
+/* Copyright [2015] [Alessandro Settimi]
+ * 
+ * email: [email protected]
+ * 
+ * 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 GAZEBO_ROS_IMU_SENSOR_H
+#define GAZEBO_ROS_IMU_SENSOR_H
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/common/UpdateInfo.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Pose3.hh>
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <string>
+
+namespace gazebo
+{
+  namespace sensors
+  {
+    class ImuSensor;
+  }
+  /**
+  @anchor GazeboRosAccelSensor
+  \ref GazeboRosAccelSensor is a plugin to simulate an Acceleration sensor, the main differences from \ref GazeboRosIMU are:
+  - inheritance from SensorPlugin instead of ModelPlugin,
+  - measurements are given by gazebo ImuSensor instead of being computed by the ros plugin,
+  - gravity is included in inertial measurements.
+  */
+  /** @brief Gazebo Ros accel sensor plugin. */
+  class GazeboRosAccelSensor : public SensorPlugin
+  {
+  public:
+    /// \brief Constructor.
+    GazeboRosAccelSensor();
+    /// \brief Destructor.
+    virtual ~GazeboRosAccelSensor();
+    /// \brief Load the sensor.
+    /// \param sensor_ pointer to the sensor.
+    /// \param sdf_ pointer to the sdf config file.
+    virtual void Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_);
+
+  protected:
+    /// \brief Update the sensor.
+    virtual void UpdateChild(const gazebo::common::UpdateInfo &/*_info*/);
+
+  private:
+    /// \brief Load the parameters from the sdf file.
+    bool LoadParameters();
+    /// \brief Gaussian noise generator.
+    /// \param mu offset value.
+    /// \param sigma scaling value.
+    double GuassianKernel(double mu, double sigma);
+
+    /// \brief Ros NodeHandle pointer.
+    ros::NodeHandle* node;
+    /// \brief Ros Publisher for imu data.
+    ros::Publisher imu_data_publisher;
+    /// \brief Ros IMU message.
+    sensor_msgs::Imu imu_msg;
+
+    /// \brief last time on which the data was published.
+    common::Time last_time;
+    /// \brief Pointer to the update event connection.
+    gazebo::event::ConnectionPtr connection;
+    /// \brief Pointer to the sensor.
+    sensors::ImuSensor* sensor;
+    /// \brief Pointer to the sdf config file.
+    sdf::ElementPtr sdf;
+    /// \brief Orientation data from the sensor.
+    //ignition::math::Quaterniond orientation;
+    /// \brief Linear acceleration data from the sensor.
+    ignition::math::Vector3d accelerometer_data;
+    /// \brief Angular velocity data from the sensor.
+    //ignition::math::Vector3d gyroscope_data;
+
+    /// \brief Seed for the Gaussian noise generator.
+    unsigned int seed;
+
+    //loaded parameters
+    /// \brief The data is published on the topic named: /robot_namespace/topic_name.
+    std::string robot_namespace;
+    /// \brief The data is published on the topic named: /robot_namespace/topic_name.
+    std::string topic_name;
+    /// \brief Name of the link of the IMU.
+    std::string body_name;
+    /// \brief Sensor update rate.
+    double update_rate;
+    /// \brief Gaussian noise.
+    double gaussian_noise;
+    /// \brief Offset parameter, position part is unused.
+    ignition::math::Pose3d offset;
+  };
+}
+
+#endif //GAZEBO_ROS_IMU_SENSOR_H

include/realsense_gazebo_plugin/gazebo_ros_gyro_sensor.h

@@ -0,0 +1,107 @@
+/* Copyright [2015] [Alessandro Settimi]
+ * 
+ * email: [email protected]
+ * 
+ * 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 GAZEBO_ROS_IMU_SENSOR_H
+#define GAZEBO_ROS_IMU_SENSOR_H
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/common/UpdateInfo.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Pose3.hh>
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <string>
+
+namespace gazebo
+{
+  namespace sensors
+  {
+    class ImuSensor;
+  }
+  /**
+  @anchor GazeboRosgyroSensor
+  \ref GazeboRosgyroSensor is a plugin to simulate a gyroscope sensor, the main differences from \ref GazeboRosIMU are:
+  - inheritance from SensorPlugin instead of ModelPlugin,
+  - measurements are given by gazebo ImuSensor instead of being computed by the ros plugin,
+  - gravity is included in inertial measurements.
+  */
+  /** @brief Gazebo Ros gyroscope sensor plugin. */
+  class GazeboRosgyroSensor : public SensorPlugin
+  {
+  public:
+    /// \brief Constructor.
+    GazeboRosgyroSensor();
+    /// \brief Destructor.
+    virtual ~GazeboRosgyroSensor();
+    /// \brief Load the sensor.
+    /// \param sensor_ pointer to the sensor.
+    /// \param sdf_ pointer to the sdf config file.
+    virtual void Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_);
+
+  protected:
+    /// \brief Update the sensor.
+    virtual void UpdateChild(const gazebo::common::UpdateInfo &/*_info*/);
+
+  private:
+    /// \brief Load the parameters from the sdf file.
+    bool LoadParameters();
+    /// \brief Gaussian noise generator.
+    /// \param mu offset value.
+    /// \param sigma scaling value.
+    double GuassianKernel(double mu, double sigma);
+
+    /// \brief Ros NodeHandle pointer.
+    ros::NodeHandle* node;
+    /// \brief Ros Publisher for imu data.
+    ros::Publisher imu_data_publisher;
+    /// \brief Ros IMU message.
+    sensor_msgs::Imu imu_msg;
+
+    /// \brief last time on which the data was published.
+    common::Time last_time;
+    /// \brief Pointer to the update event connection.
+    gazebo::event::ConnectionPtr connection;
+    /// \brief Pointer to the sensor.
+    sensors::ImuSensor* sensor;
+    /// \brief Pointer to the sdf config file.
+    sdf::ElementPtr sdf;
+    /// \brief Orientation data from the sensor.
+    //ignition::math::Quaterniond orientation;
+    /// \brief Linear acceleration data from the sensor.
+    //ignition::math::Vector3d accelerometer_data;
+    /// \brief Angular velocity data from the sensor.
+    ignition::math::Vector3d gyroscope_data;
+
+    /// \brief Seed for the Gaussian noise generator.
+    unsigned int seed;
+
+    //loaded parameters
+    /// \brief The data is published on the topic named: /robot_namespace/topic_name.
+    std::string robot_namespace;
+    /// \brief The data is published on the topic named: /robot_namespace/topic_name.
+    std::string topic_name;
+    /// \brief Name of the link of the IMU.
+    std::string body_name;
+    /// \brief Sensor update rate.
+    double update_rate;
+    /// \brief Gaussian noise.
+    double gaussian_noise;
+    /// \brief Offset parameter, position part is unused.
+    ignition::math::Pose3d offset;
+  };
+}
+
+#endif //GAZEBO_ROS_IMU_SENSOR_H