Merge branch 'main' into unstable-odrive

.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_ros2:
     type: git
     url: https://github.com/clubcapra/ovis_ros2
@@ -15,3 +11,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')
@@ -20,7 +23,18 @@ def generate_launch_description():
     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": use_sim_time, }
+        ]
+    )
 
     # Visualize in RViz
     rviz = Node(
@@ -33,39 +47,37 @@ def generate_launch_description():
     # 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}]
-                   )
+        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': use_sim_time}],
+        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/filtered/global")],
-    )
+        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': use_sim_time}],
+        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}],
+                   {'use_sim_time': use_sim_time}],
         remappings=[
             # Subscribed Topics
                     ("imu/data", "imu"),
                     ("gps/fix", "gps"),
-                    ("odometry/filtered", "odometry/filtered/global"),
-            # Published Topics
-                    ("gps/filtered", "gps/filtered"),
-                    ("odometry/gps", "odometry/gps"),
+                    ('odometry/filtered', 'odometry/global'),
                 ],
     )
 
@@ -80,7 +92,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

@@ -61,6 +61,9 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_bringup, "launch", "common.launch.py"),
         ),
+        launch_arguments={
+            "use_sim_time": "false",
+        }.items(),
     )
 
     ###### ROS2 control ######
@@ -140,11 +143,19 @@ def create_controller_node(node_name:str):
     #     )
     # )
 
+    velodyne = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"),
+        ),
+    )
+
     return LaunchDescription([
             robot_state_pub_node,
             control_node,
             common,
             joint_state_broadcaster_spawner,
             *delayed_controller_nodes,
             # vectornav,
+            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([