Fix depth camera for Gazebo Ignition.

diff --git a/twil_2dnav/config/twil_2dnav.rviz b/twil_2dnav/config/twil_2dnav.rviz
index 9fedbe4..ebbeceb 100644 (file)
--- a/twil_2dnav/config/twil_2dnav.rviz
+++ b/twil_2dnav/config/twil_2dnav.rviz
@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz_common/Displays
 Panels:
   - Class: rviz_common/Displays

-    Help Height: 70
+    Help Height: 104

     Name: Displays
     Property Tree Widget:
       Expanded:
     Name: Displays
     Property Tree Widget:
       Expanded:


@@ -8,7 +8,7 @@ Panels:
         - /Global Plan1/Topic1
         - /Camera Color1/Visibility1
       Splitter Ratio: 0.570588231086731
         - /Global Plan1/Topic1
         - /Camera Color1/Visibility1
       Splitter Ratio: 0.570588231086731

-    Tree Height: 149
+    Tree Height: 161

   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties


@@ -222,6 +222,8 @@ Visualization Manager:
       Visual Enabled: true
     - Class: rviz_default_plugins/TF
       Enabled: false
       Visual Enabled: true
     - Class: rviz_default_plugins/TF
       Enabled: false

+      Filter (blacklist): ""
+      Filter (whitelist): ""

       Frame Timeout: 15
       Frames:
         All Enabled: false
       Frame Timeout: 15
       Frames:
         All Enabled: false


@@ -333,6 +335,8 @@ Visualization Manager:
         Value: /twist_mrac_linearizing_controller/odom
       Value: true
     - Alpha: 0.699999988079071
         Value: /twist_mrac_linearizing_controller/odom
       Value: true
     - Alpha: 0.699999988079071

+      Binary representation: false
+      Binary threshold: 100

       Class: rviz_default_plugins/Map
       Color Scheme: map
       Draw Behind: false
       Class: rviz_default_plugins/Map
       Color Scheme: map
       Draw Behind: false


@@ -354,6 +358,8 @@ Visualization Manager:
       Use Timestamp: false
       Value: true
     - Alpha: 0.699999988079071
       Use Timestamp: false
       Value: true
     - Alpha: 0.699999988079071

+      Binary representation: false
+      Binary threshold: 100

       Class: rviz_default_plugins/Map
       Color Scheme: costmap
       Draw Behind: false
       Class: rviz_default_plugins/Map
       Color Scheme: costmap
       Draw Behind: false


@@ -375,6 +381,8 @@ Visualization Manager:
       Use Timestamp: false
       Value: true
     - Alpha: 0.699999988079071
       Use Timestamp: false
       Value: true
     - Alpha: 0.699999988079071

+      Binary representation: false
+      Binary threshold: 100

       Class: rviz_default_plugins/Map
       Color Scheme: map
       Draw Behind: false
       Class: rviz_default_plugins/Map
       Color Scheme: map
       Draw Behind: false


@@ -549,7 +557,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable

-        Value: /camera/depth/image_raw
+        Value: /depth_camera/image_raw

       Value: true
       Visibility:
         Camera Color: false
       Value: true
       Visibility:
         Camera Color: false


@@ -596,10 +604,9 @@ Visualization Manager:
       Topic:
         Depth: 5
         Durability Policy: Volatile
       Topic:
         Depth: 5
         Durability Policy: Volatile

-        Filter size: 10

         History Policy: Keep Last
         Reliability Policy: Reliable
         History Policy: Keep Last
         Reliability Policy: Reliable

-        Value: /camera/depth/points
+        Value: /depth_camera/point_cloud

       Use Fixed Frame: true
       Use rainbow: true
       Value: true
       Use Fixed Frame: true
       Use rainbow: true
       Value: true


@@ -676,10 +683,10 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
     collapsed: false
   Displays:
     collapsed: false

-  Height: 895
+  Height: 1025

   Hide Left Dock: false
   Hide Right Dock: false
   Hide Left Dock: false
   Hide Right Dock: false

-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002e1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000118000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001800430061006d006500720061002000440065007000740068010000015b000000d90000002800fffffffb0000001800430061006d00650072006100200043006f006c006f0072010000023a000000e40000002800ffffff000000010000010f000002e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002e1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d00650100000000000005000000026300fffffffb0000000800540069006d006501000000000000045000000000000000000000028f000002e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016b00000332fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000560000015f0000012600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001800430061006d00650072006100200044006500700074006801000001be000000db0000001e00fffffffb0000001800430061006d00650072006100200043006f006c006f007201000002a2000000e60000001e00ffffff000000010000010f00000332fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000005600000332000000ec00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006aa0000004afc0100000002fb0000000800540069006d00650100000000000006aa0000037a00fffffffb0000000800540069006d006501000000000000045000000000000000000000041e0000033200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000

   Selection:
     collapsed: false
   Time:
   Selection:
     collapsed: false
   Time:


@@ -688,6 +695,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
     collapsed: false
   Views:
     collapsed: false

-  Width: 1280
-  X: 640
+  Width: 1706
+  X: 854

   Y: 0
   Y: 0

diff --git a/twil_description/config/ros_gz_bridge.yaml b/twil_description/config/ros_gz_bridge.yaml
index 57b38d9..8c82c8e 100644 (file)
--- a/twil_description/config/ros_gz_bridge.yaml
+++ b/twil_description/config/ros_gz_bridge.yaml
@@ -48,14 +48,20 @@
   direction: GZ_TO_ROS
 
 - ros_topic_name: "/depth_camera/camera_info"
   direction: GZ_TO_ROS
 
 - ros_topic_name: "/depth_camera/camera_info"

-  gz_topic_name: "/depth_camera/camera_info"
+  gz_topic_name: "/camera_info"

   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "gz.msgs.CameraInfo"
   direction: GZ_TO_ROS
 
 - ros_topic_name: "/depth_camera/image_raw"
   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "gz.msgs.CameraInfo"
   direction: GZ_TO_ROS
 
 - ros_topic_name: "/depth_camera/image_raw"

-  gz_topic_name: "/depth_camera/image_raw"
+  gz_topic_name: "/depth_camera"

   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "gz.msgs.Image"
   direction: GZ_TO_ROS
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "gz.msgs.Image"
   direction: GZ_TO_ROS

+
+- ros_topic_name: "/depth_camera/point_cloud"
+  gz_topic_name: "/depth_camera/points"
+  ros_type_name: "sensor_msgs/msg/PointCloud2"
+  gz_type_name: "gz.msgs.PointCloudPacked"
+  direction: GZ_TO_ROS

   
\ No newline at end of file
   
\ No newline at end of file

diff --git a/twil_description/doc/realsense.txt b/twil_description/doc/realsense.txt
new file mode 100644 (file)
index 0000000..1d6135b
--- /dev/null
+++ b/twil_description/doc/realsense.txt
@@ -0,0 +1 @@
+sudo apt install ros-<ROS_DISTRO>-realsense2-*

diff --git a/twil_description/xacro/d435.urdf.xacro b/twil_description/xacro/d435.urdf.xacro
index 64bad23..8609468 100644 (file)
--- a/twil_description/xacro/d435.urdf.xacro
+++ b/twil_description/xacro/d435.urdf.xacro
@@ -23,15 +23,15 @@
                </joint>
 
                <gazebo reference="camera_link">
                </joint>
 
                <gazebo reference="camera_link">

-                       <sensor name="camera" type="depth">
-                       <!--sensor name="camera" type="depth_camera"-->
+                       <sensor name="camera" type="depth_camera">

                                <always_on>true</always_on>
                                <update_rate>15</update_rate>   <!--or 6 or 30 -->
                                <visualize>false</visualize>
                                <always_on>true</always_on>
                                <update_rate>15</update_rate>   <!--or 6 or 30 -->
                                <visualize>false</visualize>

-                               <triggered>false</triggered>
-                               <trigger_topic>trigger</trigger_topic>
+                               <!--triggered>false</triggered-->
+                               <!--trigger_topic>trigger</trigger_topic-->

                                <topic>depth_camera</topic>     <!-- This is only used by Gazebo Ignition -->
                                <topic>depth_camera</topic>     <!-- This is only used by Gazebo Ignition -->

-                               <ignition_frame_id>camera_optical_frame</ignition_frame_id>
+                               <frame_id>camera_optical_frame</frame_id>
+                               <gz_frame_id>camera_optical_frame</gz_frame_id>

                                <camera>
                                        <horizontal_fov>1.309</horizontal_fov>  <!--75 deg at 480p  or 87 deg at 720p-->
                                        <image>
                                <camera>
                                        <horizontal_fov>1.309</horizontal_fov>  <!--75 deg at 480p  or 87 deg at 720p-->
                                        <image>


@@ -50,37 +50,16 @@
                                                <stddev>0.02</stddev>
                                        </noise>
                                </camera>
                                                <stddev>0.02</stddev>
                                        </noise>
                                </camera>

-
-                               <!-- Gazebo Classic -->
-                               <xacro:if value="${hardware == 'gazebo'}">
-                                       <plugin name="camera_plugin" filename="libgazebo_ros_camera.so">
-                                               <baseline>0.05</baseline>       <!-- dist between sensors -->
-                                               <!-- Keep this zero, update_rate in the parent <sensor> tag 
-                                               will control the frame rate. -->
-                                               <update_rate>0.0</update_rate>
-                                               <camera_name>camera</camera_name>
-                                               <frame_name>camera_optical_frame</frame_name>
-                                               <hack_baseline>0.05</hack_baseline>
-                                               <!--P_cx>0.0</P_cx?
-                                               <P_cy>0.0</P_cy>
-                                               <P_fy>0.0</P_fy>
-                                               <Tx>0.0</Tx>
-                                               <Ty>0.0</Ty>
-                                               <rectification_matrix>1 0 0 0 1 0 0 0 1</rectification_matrix-->
-                                               <ros>
-                                                       <!--namespace>namespace</namespace-->
-                                                       <remapping>/camera/image_raw:=/camera/color/image_raw</remapping>
-                                                       <remapping>/camera/camera_info:=camera/color/camera_info</remapping>
-                                                       <remapping>/camera/depth/image_raw:=/camera/depth/image_raw</remapping>
-                                                       <remapping>/camera_depth/camera_info:=/camera/depth/camera_info</remapping>
-                                                       <remapping>/camera/points:=camera/depth/points</remapping>
-                                               </ros>
-                                               <!--pointCloudCutoff>0.175</pointCloudCutoff--> <!-- or 0.28 at 720p -->
-                                               <min_depth>0.05</min_depth>
-                                               <max_depth>10</max_depth>
-                                       </plugin>
-                               </xacro:if>

                        </sensor>
                </gazebo>
                        </sensor>
                </gazebo>

+
+               <!-- Gazebo Sim-->
+               <gazebo>
+                       <xacro:if value="${hardware == 'gazebo'}">
+                               <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
+                                       <render_engine>ogre2</render_engine>
+                               </plugin>
+                       </xacro:if>                     
+               </gazebo>

        </xacro:if>
 </robot>
        </xacro:if>
 </robot>

diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index 2f25bbc..5d2d07a 100644 (file)
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -151,8 +151,8 @@
        </joint>
   </xacro:if>
 
        </joint>
   </xacro:if>
 

-<!--  <xacro:if value="$(arg d435)">
+  <xacro:if value="$(arg d435)">

        <xacro:include filename="d435.urdf.xacro"/>
   </xacro:if>
        <xacro:include filename="d435.urdf.xacro"/>
   </xacro:if>

- -->
+

 </robot>
 </robot>