corrections for the multiple cameras case

include/realsense_gazebo_plugin/RealSensePlugin.h

@@ -46,7 +46,7 @@ namespace gazebo
     public: ~RealSensePlugin();
 
     // Documentation Inherited.
-    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf, std::string prefix="");
 
     /// \brief Callback for the World Update event.
     public: void OnUpdate();

include/realsense_gazebo_plugin/gazebo_ros_realsense.h

@@ -42,6 +42,7 @@ namespace gazebo
     /// \brief A pointer to the ROS node.
     ///  A node will be instantiated if it does not exist.
     protected: ros::NodeHandle* rosnode_;
+    public: std::string prefix;
     private: image_transport::ImageTransport* itnode_;
     protected: image_transport::CameraPublisher color_pub_, ir1_pub_, ir2_pub_, depth_pub_;
 

launch/two_realsense_chessboard.rviz

@@ -0,0 +1,174 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 278
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+      Splitter Ratio: 0.6350210905075073
+    Tree Height: 1317
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Camera
+Preferences:
+  PromptSaveOnExit: true
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /left_rs200/camera/color/image_raw
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+      Visibility:
+        Camera: true
+        Grid: true
+        TF: true
+        Value: true
+      Zoom Factor: 1
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        left_rs200_color:
+          Value: true
+        right_rs200_color:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 0.30000001192092896
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          left_rs200_color:
+            {}
+          right_rs200_color:
+            {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /right_rs200/camera/color/image_raw
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+      Visibility:
+        Camera: true
+        Grid: true
+        TF: true
+        Value: true
+      Zoom Factor: 1
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.4991731643676758
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.008180774748325348
+        Y: -0.010322336107492447
+        Z: 0.20774713158607483
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5997961759567261
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 2.9690325260162354
+    Saved: ~
+Window Geometry:
+  "&Camera":
+    collapsed: false
+  "&Displays":
+    collapsed: false
+  "&Time":
+    collapsed: false
+  C&amera:
+    collapsed: false
+  Height: 2027
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000473000006b8fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008001000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000073000003430000003601000003fb0000000c00430061006d00650072006101000003b700000374000000360100000300000001000003bc000006b8fc0200000006fb000000100044006900730070006c0061007900730100000073000006b80000011301000003fb0000000a005600690065007700730000000044000003ab0000011201000003fb0000000c00430061006d006500720061010000019a000001550000000000000000fb0000000c00430061006d006500720061010000016d000001820000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000e3fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000f000000007cfc0100000002fb0000000800540069006d0065010000000000000f000000052101000003fb0000000800540069006d00650100000000000004500000000000000000000006cf000006b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 3840
+  X: 0
+  Y: 0

launch/two_realsense_chessboard_gazebo_spawn.launch

@@ -0,0 +1,32 @@
+<?xml version="1.0" ?>
+<launch>
+
+  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find gazebo_ros)/worlds/empty.world"/>  
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="headless" value="$(arg headless)"/>
+  </include>
+
+
+  <node name="chessboard_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
+    args="-database chessboard2 -sdf -model chessboard22 -x 0.0 -y 0.0 -z 0.02 -R 0.0 -P 0.0 -Y 1.570791"/>
+
+  <param name="robot_description" command="$(find xacro)/xacro $(find realsense_gazebo_plugin)/urdf/chessboard_cameras_cell.urdf.xacro"/>
+
+  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
+    args="-urdf -model two_realsense -param robot_description"/>
+
+
+
+</launch>

launch/two_realsense_chessboard_rviz.launch

@@ -0,0 +1,21 @@
+<?xml version="1.0" ?>
+<launch>    
+
+  <!-- q= has the rpy set in the chessboard_cameras_cell.urdf.xacro file as rpy=0 1.061379 1.570791-->
+  <!-- q3=q*roty(pi/2)*rotz(-pi/2) rotations needed between the sensor frame and the optical camera frame - they should be included in the model but are not...-->
+    <!-- MATLAB> q=UnitQuaternion.rpy([0 , 1.061379 , 1.570791])-->
+    <!-- MATLAB> q3=q*UnitQuaternion(roty(pi/2))*UnitQuaternion(rotz(-pi/2)) -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="camera_left_broadcaster" args="-0.06 -0.29 0.410304  0.0 0.0000 -2.632175326794897 /world /left_rs200_color" /> 
+  <node pkg="tf2_ros" type="static_transform_publisher" name="camera_right_broadcaster" args="0.06 0.29 0.410304  0 3.1416 0.5236  /world /right_rs200_color" /> 
+
+  <arg name="rviz"  default="true"/>
+  <!-- marker detector -->
+  <!-- aruco_ros node is used to detect markers -->
+  <!-- ros wiki page: http://wiki.ros.org/aruco_ros -->
+  <!-- external page: http://www.uco.es/investiga/grupos/ava/node/26 -->  
+
+
+
+  <!-- Show in Rviz   -->   
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find realsense_gazebo_plugin)/launch/two_realsense_chessboard.rviz"/> 
+</launch>

models/chessboard2/chessboard2.sdf

@@ -0,0 +1,65 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="chessboard2">
+    <static>0</static>
+    <link name="link">
+
+      <inertial>
+        <mass>0.05</mass>
+        <inertia>
+          <ixx>0.000015</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.000015</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.0000108</izz>
+        </inertia>
+      </inertial>
+
+      <collision name="colision">
+        <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.46 0.46 0.04</size>
+            </box>
+          </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>30.0</mu>
+              <mu2>30.0</mu2>
+              <fdir1>0 0 0</fdir1>
+            </ode>
+          </friction>
+          <contact>
+            <ode>
+              <kp>1000000.0</kp>
+              <kd>100.0</kd>
+              <min_depth>0.0025</min_depth>
+              <max_vel>1.0</max_vel>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+
+      <visual name="visual1">
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+             <uri>model://chessboard2/meshes/chessboard2.dae</uri>
+    		<!-- the dae is a cube of 5x5x5 cm and the chessboard is 46x46x4-->
+	    <scale> 9.2 9.2 0.8 </scale>
+          </mesh>
+        </geometry>
+      </visual>
+
+      <velocity_decay>
+        <linear>0.000000</linear>
+        <angular>0.000000</angular>
+      </velocity_decay>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>1</gravity>
+    </link>
+  </model>
+</sdf>