Merge pull request #22 from mleegwt/sensors

Sensors

Author: mleegwt

ros_ws/src/navigation/config/karaburan_boat.sdf

@@ -15,7 +15,7 @@
         <pose>0 0 0.1 0 0 0</pose>
       </visual>
 
-      <!-- Massa/inertie van de romp; COM iets lager dan midden (0.07) -->
+      <!-- Massa/inertie van de romp; COM iets lager dan midden -->
       <inertial>
         <pose>0 0 -0.02 0 0 0</pose>
         <mass>4.0</mass>
@@ -30,6 +30,20 @@
         <linear>0.02</linear>
         <angular>0.15</angular>
       </velocity_decay>
+
+      <sensor name="imu_sensor" type="imu">
+        <gz_frame_id>imu_link</gz_frame_id>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <visualize>false</visualize>
+        <topic>/imu</topic>
+      </sensor>
+      <sensor name="gps_sensor" type="navsat">
+        <gz_frame_id>gps_link</gz_frame_id>
+        <always_on>1</always_on>
+        <update_rate>5</update_rate>
+        <topic>/gps</topic>
+      </sensor>
     </link>
 
       <!-- LEFT PROPELLER (link + joint) -->

ros_ws/src/navigation/config/world.sdf

@@ -30,6 +30,18 @@
         name="gz::sim::systems::Imu">
     </plugin>
 
+    <!-- Vereist door Gazebo's NavSat-systeem -->
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <world_frame_orientation>ENU</world_frame_orientation>
+      <latitude_deg>52.3728</latitude_deg>   <!-- bv. Amsterdam -->
+      <longitude_deg>4.8936</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+
+    <plugin filename="gz-sim-navsat-system" name="gz::sim::systems::NavSat"/>
+
     <model name="water_surface">
       <static>true</static>
       <pose>0 0 0 0 0 0</pose>

ros_ws/src/navigation/launch/sim.launch.py

@@ -11,7 +11,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, PythonExpression
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
@@ -31,6 +31,15 @@ def generate_launch_description():
     with_gazebo = LaunchConfiguration("with_gazebo")
     xacro_args = LaunchConfiguration("xacro_args")
 
+    # Build parameter_bridge arguments (GZ -> ROS for sensors)
+    ns = LaunchConfiguration('ns')
+    imu_topic = LaunchConfiguration('imu_topic')
+    gps_topic = LaunchConfiguration('gps_topic')
+    publish_static_tf = LaunchConfiguration('publish_static_tf')
+    base_frame = LaunchConfiguration('base_frame')
+    imu_frame = LaunchConfiguration('imu_frame')
+    gps_frame = LaunchConfiguration('gps_frame') 
+
     return LaunchDescription([
         # Paths / args
         DeclareLaunchArgument(
@@ -59,6 +68,13 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "xacro_args", default_value="",
         ),
+        # Launch args for the GZ - ROS2 bridge
+        DeclareLaunchArgument('ns', default_value='', description='ROS namespace for the bridge'),
+        DeclareLaunchArgument('imu_topic', default_value='/imu', description='GZ/ROS topic for IMU'),
+        DeclareLaunchArgument('gps_topic', default_value='/gps', description='GZ/ROS topic for GPS/NavSat'),
+        DeclareLaunchArgument('base_frame', default_value='base_link', description='Base frame id'),
+        DeclareLaunchArgument('imu_frame', default_value='imu_link', description='IMU frame id (must match gz_frame_id in SDF)'),
+        DeclareLaunchArgument('gps_frame', default_value='gps_link', description='GPS frame id (must match gz_frame_id in SDF)'),
 
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(PathJoinSubstitution([
@@ -98,6 +114,18 @@ def generate_launch_description():
                         "x": x, "y": y, "z": z,
                         "R": R, "P": P, "Y": Y
                     }.items()
+                ),
+                Node(
+                    package='ros_gz_bridge',
+                    executable='parameter_bridge',
+                    name='ros_gz_parameter_bridge',
+                    namespace=ns,
+                    output='screen',
+                    arguments=[
+                        PythonExpression(['"', imu_topic, '" + \'@sensor_msgs/msg/Imu[gz.msgs.IMU\'']),
+                        PythonExpression(['"', gps_topic, '" + \'@sensor_msgs/msg/NavSatFix[gz.msgs.NavSat\'']),
+                        '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'
+                    ],
                 )
             ]
         ),