Add more examples, need to debug some

gazebo_plugins/worlds/gazebo_ros_ray_sensor_demo.world

@@ -1,18 +1,39 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!--
   Gazebo ROS Ray Sensor plugin demo
+
   To run:
-  gzserver gazebo_ros_ray_sensor_demo.world
+
+      gazebo gazebo_ros_ray_sensor_demo.world
+
   Then open RVIZ:
-  ros2 run rviz2 rviz2
-  And add the topic /ray/pointcloud2
+
+      ros2 run rviz2 rviz2
+
+  On RViz, add the following topics:
+
+      * /ray/pointcloud2
+      * /ray/pointcloud
+      * /ray/laserscan
+      * /gpu_ray/pointcloud2
+      * /gpu_ray/pointcloud
+      * /gpu_ray/laserscan
+
+  On RViz, change the "Fixed Frame" to `ray_link`
+
+  Echo the range topics, i.e.
+
+     ros2 topic echo /ray/range
+     ros2 topic echo /gpu_ray/range
 -->
 <sdf version="1.6">
   <world name="ray_sensor_demo">
     <gravity>0 0 0</gravity>
+
     <include>
       <uri>model://sun</uri>
     </include>
+
     <model name="ray_sensor_box">
       <pose>-1.5 0.0 0.0 0.0 0.0 0.0</pose>
       <link name="ray_link">
@@ -31,8 +52,9 @@
           </geometry>
           <laser_retro>100.0</laser_retro>
         </collision>
+
+        <!-- ray sensor -->
         <sensor name="sensor_ray" type="ray">
-          <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
           <ray>
             <scan>
               <horizontal>
@@ -63,9 +85,97 @@
             </ros>
             <output_type>sensor_msgs/PointCloud2</output_type>
           </plugin>
+          <plugin name="pc" filename="libgazebo_ros_ray_sensor.so">
+            <ros>
+              <node_name>pc</node_name>
+              <namespace>/ray</namespace>
+              <argument>~/out:=pointcloud</argument>
+            </ros>
+            <output_type>sensor_msgs/PointCloud</output_type>
+          </plugin>
+          <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
+            <ros>
+              <node_name>laserscan</node_name>
+              <namespace>/ray</namespace>
+              <argument>~/out:=laserscan</argument>
+            </ros>
+            <output_type>sensor_msgs/LaserScan</output_type>
+          </plugin>
+          <plugin name="range" filename="libgazebo_ros_ray_sensor.so">
+            <ros>
+              <node_name>range</node_name>
+              <namespace>/ray</namespace>
+              <argument>~/out:=range</argument>
+            </ros>
+            <output_type>sensor_msgs/Range</output_type>
+          </plugin>
+        </sensor>
+
+        <!-- gpu_ray sensor producing PointCloud / LaserScan -->
+        <sensor name="sensor_gpu_ray" type="gpu_ray">
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>300</samples>
+                <resolution>1.0</resolution>
+                <min_angle>-0.5236</min_angle>
+                <max_angle>0.5236</max_angle>
+              </horizontal>
+              <vertical>
+                <!--
+                  gpu_ray currently only supports 1 vertical ray
+                  See https://bitbucket.org/osrf/gazebo/issues/2486
+                -->
+                <samples>1</samples>
+                <resolution>1.0</resolution>
+                <min_angle>-0.5236</min_angle>
+                <max_angle>0.5236</max_angle>
+              </vertical>
+            </scan>
+            <range>
+              <min>0.05</min>
+              <max>50.0</max>
+            </range>
+          </ray>
+          <always_on>true</always_on>
+          <update_rate>1.0</update_rate>
+          <plugin name="pc2" filename="libgazebo_ros_ray_sensor.so">
+            <ros>
+              <node_name>pc2</node_name>
+              <namespace>/gpu_ray</namespace>
+              <argument>~/out:=pointcloud2</argument>
+            </ros>
+            <output_type>sensor_msgs/PointCloud2</output_type>
+          </plugin>
+          <plugin name="pc" filename="libgazebo_ros_ray_sensor.so">
+            <ros>
+              <node_name>pc</node_name>
+              <namespace>/gpu_ray</namespace>
+              <argument>~/out:=pointcloud</argument>
+            </ros>
+            <output_type>sensor_msgs/PointCloud</output_type>
+          </plugin>
+          <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
+            <ros>
+              <node_name>laserscan</node_name>
+              <namespace>/gpu_ray</namespace>
+              <argument>~/out:=laserscan</argument>
+            </ros>
+            <output_type>sensor_msgs/LaserScan</output_type>
+          </plugin>
+          <plugin name="range" filename="libgazebo_ros_ray_sensor.so">
+            <ros>
+              <node_name>range</node_name>
+              <namespace>/gpu_ray</namespace>
+              <argument>~/out:=range</argument>
+            </ros>
+            <output_type>sensor_msgs/Range</output_type>
+          </plugin>
         </sensor>
+
       </link>
     </model>
+
     <model name="box">
       <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
       <link name="base_link">
@@ -86,6 +196,7 @@
         </collision>
       </link>
     </model>
+
     <model name="sphere">
       <pose>0.5 0.5 0.0 0.0 0.0 0.0</pose>
       <link name="base_link">
@@ -106,6 +217,7 @@
         </collision>
       </link>
     </model>
+
     <model name="cylinder">
       <pose>0.5 -0.5 0.0 0.0 0.0 0.0</pose>
       <link name="base_link">
@@ -128,5 +240,6 @@
         </collision>
       </link>
     </model>
+
   </world>
 </sdf>