Merge pull request #49 from clubcapra/zed

Ajout des zed launch files

.devcontainer/Dockerfile

@@ -41,16 +41,7 @@ RUN apt-get update && apt-get install -y \
 RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
 RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
 
-RUN apt update -y
-
-RUN apt install -y ros-humble-ros-gzharmonic
-
-# Adding an APT update and clean step
-RUN apt-get update && \
-    apt-get install -y --no-install-recommends apt-utils && \
-    rm -rf /var/lib/apt/lists/* && \
-    apt-get clean && \
-    apt-get update
+RUN apt update -y --fix-missing
 
 # Change user
 RUN chown $USER_UID:$USER_GID /workspace/$USERNAME
@@ -60,15 +51,20 @@ USER $USERNAME
 # Copy to preload the ros packages
 COPY ./ /workspace/$USERNAME/
 
-# Install Tesseract
-
 # Source the ROS setup file
 RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc
 
 COPY --chown=$USERNAME ./ /workspace/$USERNAME
 
+# Add needed packages
+RUN vcs import src < rove.repos
+
+
 # Rosdep update
 RUN rosdep update
 
 # Install ROS dependencies
 RUN rosdep install --from-paths src --ignore-src --rosdistro humble -y
+
+# Install gzharmonic after rosdep install
+RUN sudo apt install -y ros-humble-ros-gzharmonic

.github/workflows/build.yml

@@ -17,5 +17,5 @@ jobs:
         docker compose build --build-arg USER_UID=$(id -u) --build-arg USER_GID=$(id -g)
     - name: Build rove
       run: |
-        docker compose run devcontainer bash -c "colcon build"
+        docker compose run devcontainer bash -c "colcon build --packages-ignore zed_components zed_wrapper"
 

rove.repos

@@ -1,8 +1,4 @@
 repositories:
-  capra_gazebo:
-    type: git
-    url: https://github.com/clubcapra/capra_gazebo.git
-    version: main
   ovis_description:
     type: git
     url: https://github.com/clubcapra/ovis_ros2.git
@@ -11,3 +7,12 @@ repositories:
     type: git
     url: https://github.com/dawonn/vectornav.git
     version: ros2
+  zed_wrapper:
+    type: git
+    url: https://github.com/stereolabs/zed-ros2-wrapper.git
+    version: master
+  zed_wrapper/zed-ros2-interfaces:
+    type: git
+    url: https://github.com/stereolabs/zed-ros2-interfaces.git
+    version: main
+

src/rove_bringup/config/VLP16_velodyne_params.yaml

@@ -0,0 +1,30 @@
+# from https://github.com/ros-drivers/velodyne/tree/ros2
+velodyne_driver_node:
+    ros__parameters:
+        device_ip: 192.168.84.201 # be sure that your ip adress is 192.168.84.150/24 or look destination in the velodyne settings
+        gps_time: false
+        time_offset: 0.0
+        enabled: true
+        read_once: false
+        read_fast: false
+        repeat_delay: 0.0
+        frame_id: velodyne
+        model: VLP16
+        rpm: 600.0
+        port: 2368
+        timestamp_first_packet: false
+
+velodyne_laserscan_node:
+    ros__parameters:
+        ring: -1
+        resolution: 0.007
+
+velodyne_transform_node:
+    ros__parameters:
+        model: VLP16
+        min_range: 0.9
+        max_range: 130.0
+        view_direction: 0.0
+        fixed_frame: ""
+        target_frame: ""
+        organize_cloud: true

src/rove_bringup/config/VLP16db.yaml

@@ -0,0 +1,53 @@
+# callibration of the Velodyne lidar
+# from https://github.com/ros-drivers/velodyne/tree/ros2
+lasers:
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0,
+  vert_correction: -0.2617993877991494, vert_offset_correction: 0.0112}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0,
+  vert_correction: 0.017453292519943295, vert_offset_correction: -0.0007}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0,
+  vert_correction: -0.22689280275926285, vert_offset_correction: 0.0097}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0,
+  vert_correction: 0.05235987755982989, vert_offset_correction: -0.0022}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0,
+  vert_correction: -0.19198621771937624, vert_offset_correction: 0.0081}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0,
+  vert_correction: 0.08726646259971647, vert_offset_correction: -0.0037}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0,
+  vert_correction: -0.15707963267948966, vert_offset_correction: 0.0066}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0,
+  vert_correction: 0.12217304763960307, vert_offset_correction: -0.0051}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0,
+  vert_correction: -0.12217304763960307, vert_offset_correction: 0.0051}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0,
+  vert_correction: 0.15707963267948966, vert_offset_correction: -0.0066}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0,
+  vert_correction: -0.08726646259971647, vert_offset_correction: 0.0037}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0,
+  vert_correction: 0.19198621771937624, vert_offset_correction: -0.0081}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0,
+  vert_correction: -0.05235987755982989, vert_offset_correction: 0.0022}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0,
+  vert_correction: 0.22689280275926285, vert_offset_correction: -0.0097}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0,
+  vert_correction: -0.017453292519943295, vert_offset_correction: 0.0007}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0,
+  vert_correction: 0.2617993877991494, vert_offset_correction: -0.0112}
+num_lasers: 16
+distance_resolution: 0.002

src/rove_bringup/launch/common.launch.py

@@ -8,9 +8,12 @@
 from launch.substitutions import Command
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
+from launch.substitutions import LaunchConfiguration
 
 
 def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
     # Get the launch directory
     pkg_rove_bringup = get_package_share_directory('rove_bringup')
     pkg_rove_description = get_package_share_directory('rove_description')
@@ -30,7 +33,7 @@ def generate_launch_description():
         output='both',
         parameters=[
             {'robot_description': robot_desc},
-            {"use_sim_time": True, }
+            {"use_sim_time": use_sim_time, }
         ]
     )
 
@@ -50,7 +53,7 @@ def generate_launch_description():
         name='ekf_filter_node_local',
         output='screen',
         parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
-                    {'use_sim_time': True}],
+                    {'use_sim_time': use_sim_time}],
         remappings=[('odometry/filtered', 'odometry/local')])
 
 
@@ -60,7 +63,7 @@ def generate_launch_description():
         name='ekf_filter_node_global',
         output='screen',
         parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
-                    {'use_sim_time': True}],
+                    {'use_sim_time': use_sim_time}],
         remappings=[('odometry/filtered', 'odometry/global')])
 
 
@@ -70,7 +73,7 @@ def generate_launch_description():
         name="navsat_transform",
         output="screen",
         parameters=[os.path.join(pkg_rove_description, 'config/navsat_transform.yaml'),
-                   {'use_sim_time': True}],
+                   {'use_sim_time': use_sim_time}],
         remappings=[
             # Subscribed Topics
                     ("imu/data", "imu"),
@@ -90,7 +93,7 @@ def generate_launch_description():
             os.path.join(pkg_rove_bringup, "launch", "autonomy.launch.py"),
         ),
         launch_arguments={
-            "use_sim_time": "true",
+            'use_sim_time': use_sim_time,
             "deskewing": "true",
             "use_slam3d": "false",
         }.items(),

src/rove_bringup/launch/real.launch.py

@@ -18,6 +18,9 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_bringup, "launch", "common.launch.py"),
         ),
+        launch_arguments={
+            "use_sim_time": "false",
+        }.items(),
     )
 
 
@@ -28,7 +31,14 @@ def generate_launch_description():
         ),
     )
 
+    velodyne = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"),
+        ),
+    )
+
     return LaunchDescription([
             common,
             vectornav,
+            velodyne,
             ])

src/rove_bringup/launch/sim.launch.py

@@ -19,6 +19,8 @@ def generate_launch_description():
     # Get the URDF file
     urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro')
     robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
+
+    # Get simulation file
     world_file_name = 'worlds/base_world.world'
     world = os.path.join(pkg_rove_description, world_file_name)
 
@@ -72,6 +74,9 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_bringup, "launch", "common.launch.py"),
         ),
+        launch_arguments={
+            "use_sim_time": "true",
+        }.items(),
     )
 
     return LaunchDescription([

src/rove_bringup/launch/test.launch.py

@@ -0,0 +1,108 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    # Get the launch directory
+    pkg_rove_bringup = get_package_share_directory('rove_bringup')
+    pkg_rove_description = get_package_share_directory('rove_description')
+    pkg_rove_slam = get_package_share_directory('rove_slam')
+    bringup_pkg_path = get_package_share_directory('rove_bringup')
+
+    # Get the URDF file
+    urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro')
+    robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
+
+    # Takes the description and joint angles as inputs and publishes
+    # the 3D poses of the robot links
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='both',
+        parameters=[
+            {'robot_description': robot_desc},
+            # {"use_sim_time": True, }
+        ]
+    )
+
+    # Visualize in RViz
+    rviz = Node(
+       package='rviz2',
+       executable='rviz2',
+       arguments=[
+        '-f', 'velodyne',
+        # '-d', os.path.join(pkg_rove_description, 'config','basic.rviz')
+        ],
+    )
+
+    # used tutorial: https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html
+
+    robot_localization_node_local = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node_local',
+        output='screen',
+        parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
+                    # {'use_sim_time': True}
+                    ],
+        remappings=[('odometry/filtered', 'odometry/local')])
+
+
+    robot_localization_node_global = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node_global',
+        output='screen',
+        parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
+                    # {'use_sim_time': True}
+                    ],
+        remappings=[('odometry/filtered', 'odometry/global')])
+
+
+    navsat_transform = Node(
+        package="robot_localization",
+        executable="navsat_transform_node",
+        name="navsat_transform",
+        output="screen",
+        parameters=[os.path.join(pkg_rove_description, 'config/navsat_transform.yaml'),
+                    # {'use_sim_time': True}
+                   ],
+        remappings=[
+            # Subscribed Topics
+                    ("imu/data", "imu"),
+                    ("gps/fix", "gps"),
+                    ('odometry/filtered', 'odometry/global'),
+                ],
+    )
+
+    ###### Sensor ######
+    vectornav = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"),
+        ),
+    )
+
+    velodyne = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"),
+        ),
+    )
+
+    return LaunchDescription([
+            # robot_state_publisher,
+            robot_localization_node_local,
+            robot_localization_node_global,
+            navsat_transform,
+            vectornav,
+            velodyne,
+            rviz,
+            ])