diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 71bdd98..00ab61a 100644 --- a/.devcontainer/Dockerfile +++ b/.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 \ No newline at end of file diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index a3080a7..f969f2a 100644 --- a/.github/workflows/build.yml +++ b/.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" diff --git a/rove.repos b/rove.repos index 8a611b7..658a1cd 100644 --- a/rove.repos +++ b/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 + diff --git a/src/rove_bringup/config/VLP16_velodyne_params.yaml b/src/rove_bringup/config/VLP16_velodyne_params.yaml new file mode 100644 index 0000000..b18fc47 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/rove_bringup/config/VLP16db.yaml b/src/rove_bringup/config/VLP16db.yaml new file mode 100644 index 0000000..9680061 --- /dev/null +++ b/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 diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py index 6b18040..54429f5 100644 --- a/src/rove_bringup/launch/common.launch.py +++ b/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,23 +47,24 @@ 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", @@ -57,15 +72,12 @@ 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"), ("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(), diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index 73ee563..7580d21 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/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,6 +143,12 @@ 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, @@ -147,4 +156,6 @@ def create_controller_node(node_name:str): joint_state_broadcaster_spawner, *delayed_controller_nodes, # vectornav, + vectornav, + velodyne, ]) diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py index 8b5e118..b13a321 100644 --- a/src/rove_bringup/launch/sim.launch.py +++ b/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([ diff --git a/src/rove_bringup/launch/test.launch.py b/src/rove_bringup/launch/test.launch.py new file mode 100644 index 0000000..736754f --- /dev/null +++ b/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, + ]) diff --git a/src/rove_bringup/launch/velodyne.launch.py b/src/rove_bringup/launch/velodyne.launch.py new file mode 100644 index 0000000..3b5c85f --- /dev/null +++ b/src/rove_bringup/launch/velodyne.launch.py @@ -0,0 +1,50 @@ +import os +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +import yaml + +# from https://github.com/ros-drivers/velodyne/tree/ros2 +def generate_launch_description(): + package_name = 'rove_bringup' + dir = get_package_share_directory(package_name) + # Get params files + params = os.path.join(dir, 'config', 'VLP16_velodyne_params.yaml') + with open(params, 'r') as f: + transform_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters'] + transform_params['calibration'] = os.path.join(dir, 'config', 'VLP16db.yaml') + + driver_node = Node(package='velodyne_driver', + executable='velodyne_driver_node', + output='both', + parameters=[params]) + + driver_exit_event = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=driver_node, + on_exit=[launch.actions.EmitEvent( + event=launch.events.Shutdown())], + )) + + # laserscan + laserscan_node = Node(package='velodyne_laserscan', + executable='velodyne_laserscan_node', + name='scan', + output='both', + parameters=[params]) + + # pointcloud + transform_node = Node(package='velodyne_pointcloud', + executable='velodyne_transform_node', + name='velodyne_points', + output='both', + parameters=[transform_params]) + + return LaunchDescription([ + driver_node, + driver_exit_event, + laserscan_node, + transform_node, + ]) \ No newline at end of file diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index 2609407..6334030 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -4,12 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /RobotModel1 - - /TF1/Frames1 - - /Map1 + - /RobotModel1/Status1 Splitter Ratio: 0.5 - Tree Height: 523 + Tree Height: 517 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -49,36 +46,66 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - base_footprint: + base_link: Alpha: 1 Show Axes: false Show Trail: false - base_link: + Value: true + flipper_fl: Alpha: 1 Show Axes: false Show Trail: false Value: true - base_scan: + flipper_fr: Alpha: 1 Show Axes: false Show Trail: false Value: true - flipper_fl: + flipper_rl: Alpha: 1 Show Axes: false Show Trail: false Value: true - flipper_fr: + flipper_rr: Alpha: 1 Show Axes: false Show Trail: false Value: true - flipper_rl: + rove_end_effector: + Alpha: 1 + Show Axes: false + Show Trail: false + rove_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - flipper_rr: + rove_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_base: Alpha: 1 Show Axes: false Show Trail: false @@ -98,6 +125,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + velodyne_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Mass Properties: Inertia: false Mass: false @@ -163,11 +200,7 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - base_footprint: - Value: true base_link: - Value: false - base_scan: Value: true flipper_fl: Value: true @@ -177,16 +210,36 @@ Visualization Manager: Value: true flipper_rr: Value: true - sensor_link: - Value: true map: Value: true odom: Value: true + rove_end_effector: + Value: true + rove_link_1: + Value: true + rove_link_2: + Value: true + rove_link_3: + Value: true + rove_link_4: + Value: true + rove_link_5: + Value: true + rove_link_6: + Value: true + rove_link_base: + Value: true + sensor_link: + Value: true track_l: Value: true track_r: Value: true + velodyne_laser: + Value: true + velodyne_link: + Value: true Marker Scale: 1 Name: TF Show Arrows: true @@ -195,23 +248,32 @@ Visualization Manager: Tree: map: odom: - base_footprint: - base_link: - base_scan: - {} - flipper_fl: - {} - flipper_fr: - {} - flipper_rl: - {} - flipper_rr: - {} - sensor_link: - {} - track_l: - {} - track_r: + base_link: + flipper_fl: + {} + flipper_fr: + {} + flipper_rl: + {} + flipper_rr: + {} + rove_link_base: + rove_link_1: + rove_link_2: + rove_link_3: + rove_link_4: + rove_link_5: + rove_link_6: + rove_end_effector: + {} + sensor_link: + {} + track_l: + {} + track_r: + {} + velodyne_link: + velodyne_laser: {} Update Interval: 0 Value: true @@ -281,46 +343,7 @@ Visualization Manager: Scale: 1 Value: true Value: false - Enabled: false - Keep: 1 - Name: raw odom - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /odom - Value: false - - Angle Tolerance: 0.10000000149011612 - Class: rviz_default_plugins/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: false - Enabled: false + Enabled: true Keep: 1 Name: filtered position Position Tolerance: 0.10000000149011612 @@ -341,7 +364,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /odometry/filtered - Value: false + Value: true - Acceleration properties: Acc. vector alpha: 1 Acc. vector color: 255; 0; 0 @@ -416,25 +439,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 86.56747436523438 + Distance: 15.358924865722656 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 11.144978523254395 - Y: -2.5526533126831055 - Z: -4.807077407836914 + X: -0.7862069010734558 + Y: 0.829606294631958 + Z: -1.3278541564941406 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.1853981018066406 + Pitch: 0.900398313999176 Target Frame: Value: Orbit (rviz) - Yaw: 6.198585033416748 + Yaw: 5.728583812713623 Saved: ~ Window Geometry: Displays: @@ -442,7 +465,7 @@ Window Geometry: Height: 814 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000294fc020000000afb000000100044006900730070006c006100790073010000003b00000294000000c700fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000290fc020000000afb000000100044006900730070006c006100790073010000003d00000290000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -452,5 +475,5 @@ Window Geometry: Views: collapsed: false Width: 1103 - X: 2030 - Y: 119 + X: 817 + Y: 109 diff --git a/src/rove_description/config/navsat_transform.yaml b/src/rove_description/config/navsat_transform.yaml index a6faa62..a7669e2 100644 --- a/src/rove_description/config/navsat_transform.yaml +++ b/src/rove_description/config/navsat_transform.yaml @@ -4,9 +4,10 @@ navsat_transform: frequency: 30.0 delay: 3.0 magnetic_declination_radians: 0.0 - yaw_offset: 0.0 + yaw_offset: pi zero_altitude: true broadcast_utm_transform: true publish_filtered_gps: true use_odometry_yaw: true - wait_for_datum: false \ No newline at end of file + wait_for_datum: false + broadcast_utm_transform_as_parent_frame: false \ No newline at end of file diff --git a/src/rove_description/urdf/fake_wheels.urdf.xacro b/src/rove_description/urdf/fake_wheels.urdf.xacro new file mode 100644 index 0000000..21531e3 --- /dev/null +++ b/src/rove_description/urdf/fake_wheels.urdf.xacro @@ -0,0 +1,53 @@ + + + + + + + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + + + diff --git a/src/rove_description/urdf/gazebo.urdf.xacro b/src/rove_description/urdf/gazebo.urdf.xacro index 9c33f2d..290568c 100644 --- a/src/rove_description/urdf/gazebo.urdf.xacro +++ b/src/rove_description/urdf/gazebo.urdf.xacro @@ -28,16 +28,7 @@ 1 cmd_vel - odom - odom - base_footprint - 30 - - false - false - - /tf --> - @@ -44,11 +43,6 @@ - - - - - diff --git a/src/rove_description/urdf/sensor.urdf.xacro b/src/rove_description/urdf/sensor.urdf.xacro index 50b08b4..7f92b0b 100644 --- a/src/rove_description/urdf/sensor.urdf.xacro +++ b/src/rove_description/urdf/sensor.urdf.xacro @@ -31,23 +31,10 @@ Gazebo/Black - imu_link + sensor_link true - 200 + 50 imu - - - gaussian - - 0.0 - 0.01 - - - 0.0 - 0.01 - - - diff --git a/src/rove_description/urdf/track.urdf.xacro b/src/rove_description/urdf/track.urdf.xacro index 328db4d..19d2502 100644 --- a/src/rove_description/urdf/track.urdf.xacro +++ b/src/rove_description/urdf/track.urdf.xacro @@ -53,8 +53,8 @@ - 0.65 - 0.85 + 1 + 1 @@ -87,7 +87,7 @@ - + track_${suffix} -3.0 3.0 diff --git a/src/rove_description/urdf/velodyne.urdf.xacro b/src/rove_description/urdf/velodyne.urdf.xacro index 7c4cb8b..bb72928 100644 --- a/src/rove_description/urdf/velodyne.urdf.xacro +++ b/src/rove_description/urdf/velodyne.urdf.xacro @@ -64,10 +64,9 @@ - ${update_rate} true true - 5 + 50 scan ${name}_laser diff --git a/src/rove_description/worlds/base_world.world b/src/rove_description/worlds/base_world.world index 813dfbd..ae1681a 100644 --- a/src/rove_description/worlds/base_world.world +++ b/src/rove_description/worlds/base_world.world @@ -98,10 +98,7 @@ - + 1 1 1 1 @@ -127,10 +124,7 @@ - + 1 1 1 1 @@ -156,10 +150,7 @@ - + 1 1 1 1 @@ -185,10 +176,7 @@ - + 1 1 1 1 @@ -214,10 +202,7 @@ - + 1 1 1 1 @@ -243,10 +228,7 @@ - + 1 1 1 1 @@ -272,10 +254,7 @@ - + 1 1 1 1 @@ -301,10 +280,7 @@ - + 1 1 1 1 @@ -330,10 +306,7 @@ - + 1 1 1 1 @@ -359,10 +332,7 @@ - + 1 1 1 1 @@ -388,10 +358,7 @@ - + 1 1 1 1 @@ -417,10 +384,7 @@ - + 1 1 1 1 @@ -446,10 +410,7 @@ - + 1 1 1 1 @@ -475,10 +436,7 @@ - + 1 1 1 1 @@ -504,10 +462,7 @@ - + 1 1 1 1 @@ -533,10 +488,7 @@ - + 1 1 1 1 @@ -562,10 +514,7 @@ - + 1 1 1 1 @@ -591,10 +540,7 @@ - + 1 1 1 1 @@ -620,10 +566,7 @@ - + 1 1 1 1 @@ -649,10 +592,7 @@ - + 1 1 1 1 @@ -678,10 +618,7 @@ - + 1 1 1 1 @@ -707,10 +644,7 @@ - + 1 1 1 1 @@ -736,10 +670,7 @@ - + 1 1 1 1 @@ -765,10 +696,7 @@ - + 1 1 1 1 @@ -794,10 +722,7 @@ - + 1 1 1 1 @@ -823,10 +748,7 @@ - + 1 1 1 1 @@ -852,10 +774,7 @@ - + 1 1 1 1 @@ -881,10 +800,7 @@ - + 1 1 1 1 @@ -910,10 +826,7 @@ - + 1 1 1 1 @@ -939,10 +852,7 @@ - + 1 1 1 1 @@ -968,10 +878,7 @@ - + 1 1 1 1 @@ -997,10 +904,7 @@ - + 1 1 1 1 @@ -1026,10 +930,7 @@ - + 1 1 1 1 diff --git a/src/rove_navigation/config/rove_nav_params.yaml b/src/rove_navigation/config/rove_nav_params.yaml index af906f7..c290fad 100644 --- a/src/rove_navigation/config/rove_nav_params.yaml +++ b/src/rove_navigation/config/rove_nav_params.yaml @@ -6,7 +6,7 @@ amcl: alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "base_footprint" + base_frame_id: "base_link" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -51,7 +51,7 @@ bt_navigator: ros__parameters: use_sim_time: true global_frame: map - robot_base_frame: base_footprint + robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 @@ -180,7 +180,7 @@ global_costmap: update_frequency: 1.0 publish_frequency: 1.0 global_frame: map - robot_base_frame: base_footprint + robot_base_frame: base_link robot_radius: 0.22 # radius set and used, so no footprint points resolution: 0.05 plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] @@ -262,7 +262,7 @@ local_costmap: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom - robot_base_frame: base_footprint + robot_base_frame: base_link rolling_window: true width: 3 height: 3 @@ -375,7 +375,7 @@ recoveries_server: wait: plugin: "nav2_recoveries/Wait" global_frame: odom - robot_base_frame: base_footprint + robot_base_frame: base_link transform_timeout: 0.1 use_sim_time: true simulate_ahead_time: 2.0 diff --git a/src/rove_rtabmap/launch/rtabmap.py b/src/rove_rtabmap/launch/rtabmap.py new file mode 100644 index 0000000..bcdb9a9 --- /dev/null +++ b/src/rove_rtabmap/launch/rtabmap.py @@ -0,0 +1,77 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true +# $ ros2 param set /camera/camera depth_module.emitter_enabled 0 +# +# $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + camera_model = 'zedm' + + parameters=[{ + 'frame_id':'camera_link', + 'subscribe_stereo':True, + 'subscribe_odom_info':True, + 'wait_imu_to_init':True}] + +# ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zedm +# ros2 launch rtabmap_launch rtabmap.launch.py \ +# rtabmap_args:="--delete_db_on_start" \ +# database_path:="/media/SSD/stable/rove/src/rove_rtabmap/maps/map.db" \ +# rgb_topic:=/zed/zed_node/rgb/image_rect_color \ +# depth_topic:=/zed/zed_node/depth/depth_registered \ +# camera_info_topic:=/zed/zed_node/depth/camera_info \ +# odom_topic:=/zed/zed_node/odom \ +# imu_topic:=/zed/zed_node/imu/data \ +# visual_odometry:=false \ +# frame_id:=zed_camera_link \ +# approx_sync:=false \ +# rgbd_sync:=true \ +# approx_rgbd_sync:=false + + remappings=[ + ('rgb/image', '/%s/zed_node/rgb/image_rect_color'), + ('depth/image', "/%s/zed/zed_node/depth/depth_registered" % camera_model), + ('left/camera_info', '/%s/zed_node/left/camera_info' % camera_model), + ('right/image_rect', '/%s/zed_node/right/image_rect_color' % camera_model), + ('right/camera_info', '/%s/zed_node/right/camera_info' % camera_model)] + + return LaunchDescription([ + + # Nodes to launch + Node( + package='rtabmap_odom', executable='stereo_odometry', output='screen', + parameters=parameters, + remappings=remappings), + + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d']), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings), + + # Compute quaternion of the IMU + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/camera/imu')]), + + # The IMU frame is missing in TF tree, add it: + Node( + package='tf2_ros', executable='static_transform_publisher', output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']), + ]) \ No newline at end of file diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml index 4e7371e..ddfefe3 100644 --- a/src/rove_slam/config/ekf.yaml +++ b/src/rove_slam/config/ekf.yaml @@ -3,53 +3,51 @@ ekf_filter_node_local: ros__parameters: frequency: 30.0 two_d_mode: false - publish_acceleration: true publish_tf: true - map_frame: map odom_frame: odom - base_link_frame: base_footprint + base_link_frame: base_link world_frame: odom odom0: /odom - odom0_config: [true, true, true, - false, false, true, + odom0_config: [true, true, false, + false, false, false, + true, true, false, false, false, false, - false, false, true, false, false, false] imu0: /imu - imu0_config: [false, false, false, - true, true, true, - false, false, false, - false, false, false, - false, false, false] + imu0_config: [false, false, false, # x y z + false, false, true, # roll pitch yaw + false, false, false, # vx vy vz + false, false, true, # vroll vpitch vyaw + true, true, false] # ax ay az + imu0_queue_size: 20 ekf_filter_node_global: ros__parameters: frequency: 30.0 two_d_mode: false - publish_acceleration: true publish_tf: true - map_frame: map odom_frame: odom - base_link_frame: base_footprint - world_frame: map + base_link_frame: base_link + world_frame: gps odom0: /odom - odom0_config: [true, true, true, - false, false, true, + odom0_config: [true, true, false, + false, false, false, + true, true, false, false, false, false, - false, false, true, false, false, false] imu0: /imu - imu0_config: [false, false, false, - true, true, true, - false, false, false, - false, false, false, - false, false, false] + imu0_config: [false, false, false, # x y z + false, false, true, # roll pitch yaw + false, false, false, # vx vy vz + false, false, true, # vroll vpitch vyaw + true, true, false] # ax ay az + imu0_queue_size: 20 # description from https://github.com/cra-ros-pkg/robot_localization/blob/ros2/params/ekf.yaml # Each sensor reading updates some or all of the filter's state. These options give you greater control over which diff --git a/src/rove_slam/config/slam_config.yaml b/src/rove_slam/config/slam_config.yaml index 22b9c99..b49b173 100644 --- a/src/rove_slam/config/slam_config.yaml +++ b/src/rove_slam/config/slam_config.yaml @@ -10,7 +10,7 @@ slam_toolbox: # ROS Parameters odom_frame: odom map_frame: map - base_frame: base_footprint + base_frame: base_link scan_topic: /scan mode: mapping #localization diff --git a/src/rove_slam/setup.py b/src/rove_slam/setup.py index 5a5ec71..2a3276f 100644 --- a/src/rove_slam/setup.py +++ b/src/rove_slam/setup.py @@ -18,7 +18,7 @@ install_requires=['setuptools'], zip_safe=True, maintainer='rove', - maintainer_email='capra@etsmtl.ca', + maintainer_email='capra@ens.etsmtl.ca', description='TODO: Package description', license='TODO: License declaration', tests_require=['pytest'], diff --git a/src/rove_zed/config/zed_body_trck.yaml b/src/rove_zed/config/zed_body_trck.yaml new file mode 100644 index 0000000..e421a25 --- /dev/null +++ b/src/rove_zed/config/zed_body_trck.yaml @@ -0,0 +1,15 @@ +# Parameters to override zed_wrapper parameters +/**: + ros__parameters: + body_tracking: + bt_enabled: true # True to enable Body Tracking + model: "HUMAN_BODY_MEDIUM" # 'HUMAN_BODY_FAST', 'HUMAN_BODY_MEDIUM', 'HUMAN_BODY_ACCURATE' + body_format: "BODY_18" # 'BODY_18','BODY_34','BODY_38','BODY_70' + allow_reduced_precision_inference: false # Allow inference to run at a lower precision to improve runtime and memory usage + max_range: 20.0 # [m] Defines a upper depth range for detections + body_kp_selection: "UPPER_BODY" # 'FULL', 'UPPER_BODY' + enable_body_fitting: false # Defines if the body fitting will be applied + enable_tracking: true # Defines if the object detection will track objects across images flow + prediction_timeout_s: 3.0 # During this time [sec], the skeleton will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions + confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of skeleton key points [0,99] + minimum_keypoints_threshold: 5 # [DYNAMIC] - Minimum number of skeleton key points to be detected for a valid skeleton diff --git a/src/rove_zed/config/zed_mapping.yaml b/src/rove_zed/config/zed_mapping.yaml new file mode 100644 index 0000000..10ef04d --- /dev/null +++ b/src/rove_zed/config/zed_mapping.yaml @@ -0,0 +1,25 @@ +# Parameters to override zed_wrapper parameters +/**: + ros__parameters: + general: + camera_model: 'zedm' + camera_name: 'zedm' # usually overwritten by launch file + grab_resolution: 'VGA' # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' + pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images + grab_frame_rate: 30 # ZED SDK internal grabbing rate + mapping: + mapping_enabled: true # True to enable mapping and fused point cloud pubblication + resolution: 0.05 # maps resolution in meters [min: 0.01f - max: 0.2f] + max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] + fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud + clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection + pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference. + pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference + depth: + depth_mode: "PERFORMANCE" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) + depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] + openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] + point_cloud_freq: 30.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) + depth_confidence: 100 # [DYNAMIC] + depth_texture_conf: 100 # [DYNAMIC] + remove_saturated_areas: true # [DYNAMIC] \ No newline at end of file diff --git a/src/rove_zed/launch/zed_body_trck.launch.py b/src/rove_zed/launch/zed_body_trck.launch.py new file mode 100644 index 0000000..657b8a9 --- /dev/null +++ b/src/rove_zed/launch/zed_body_trck.launch.py @@ -0,0 +1,29 @@ +from pathlib import Path +import yaml +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + camera_model = 'zedm' + + pkg_zed_wrapper = get_package_share_directory('zed_wrapper') + launch_file_path = os.path.join(pkg_zed_wrapper, 'launch', 'zed_camera.launch.py') + + # Parse yml config file to iterable array + pkg_rove_zed = get_package_share_directory('rove_zed') + config_override_path = os.path.join(pkg_rove_zed, 'config', 'zed_body_trck.yaml') + + return LaunchDescription([ + IncludeLaunchDescription( + launch_description_source=launch_file_path, + launch_arguments=[ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` + ['camera_model', camera_model], + ['camera_name', camera_model], + ['ros_params_override_path', config_override_path] + ] + ) + ]) diff --git a/src/rove_zed/launch/zed_mapping.launch.py b/src/rove_zed/launch/zed_mapping.launch.py new file mode 100644 index 0000000..27fd6e9 --- /dev/null +++ b/src/rove_zed/launch/zed_mapping.launch.py @@ -0,0 +1,29 @@ +from pathlib import Path +import yaml +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + camera_model = 'zedm' + + pkg_zed_wrapper = get_package_share_directory('zed_wrapper') + launch_file_path = os.path.join(pkg_zed_wrapper, 'launch', 'zed_camera.launch.py') + + # Parse yml config file to iterable array + pkg_rove_zed = get_package_share_directory('rove_zed') + config_override_path = os.path.join(pkg_rove_zed, 'config', 'zed_mapping.yaml') + + return LaunchDescription([ + IncludeLaunchDescription( + launch_description_source=launch_file_path, + launch_arguments=[ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` + ['camera_model', camera_model], + ['camera_name', camera_model], + ['ros_params_override_path', config_override_path] + ] + ) + ]) \ No newline at end of file diff --git a/src/rove_zed/package.xml b/src/rove_zed/package.xml new file mode 100644 index 0000000..0cc0d4c --- /dev/null +++ b/src/rove_zed/package.xml @@ -0,0 +1,33 @@ + + + + rove_zed + 0.0.0 + Zed package for rove + rove + MIT + + + zed_wrapper + + rtabmap_odom + rtabmap_slam + rtabmap_util + rtabmap_rviz_plugins + rtabmap_slam + rtabmap_util + rtabmap_viz + imu_filter_madgwick + tf2_ros + realsense2_camera + velodyne + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/rove_zed/readme.md b/src/rove_zed/readme.md new file mode 100644 index 0000000..d4333b7 --- /dev/null +++ b/src/rove_zed/readme.md @@ -0,0 +1,35 @@ +# Setup rove_zed + +Être sûr: +1. D'avoir la zed de branchée (port usb d'en dessous, celui du dessus ne fonctionne pas) +2. D'avoir le lidar de branché, 12V + ethernet (FAIRE ATTENTIONQUE LE CABLE FONCTIONNE ET EST BIEN BRANCHÉ) +3. D'avoir CUDA 12 d'installé et que zed Diagnostics fonctionne (`/usr/local/zed/tools/ZED_Diagnostic`) +4. D'avoir le network profile mis à `LIDAR profile` (IP adress `192.168.84.150`, aller dans settings > network) + +## Pour démarrer le mapping rtabmap avec la zed: +```bash +ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zedm +ros2 launch rtabmap_launch rtabmap.launch.py \ + rtabmap_args:="--delete_db_on_start" \ + database_path:="/media/SSD/stable/rove/src/rove_rtabmap/maps/map.db" \ + rgb_topic:=/zed/zed_node/rgb/image_rect_color \ + depth_topic:=/zed/zed_node/depth/depth_registered \ + camera_info_topic:=/zed/zed_node/depth/camera_info \ + odom_topic:=/zed/zed_node/odom \ + imu_topic:=/zed/zed_node/imu/data \ + visual_odometry:=false \ + frame_id:=zed_camera_link \ + approx_sync:=false \ + rgbd_sync:=true \ + approx_rgbd_sync:=false +``` + +## Pour démarrer le body tracking: +```bash +ros2 launch rove_zed body_trck.launch.py +``` + +## Pour démarrer le lidar: +```bash +ros2 launch rove_bringup test.launch.py +``` \ No newline at end of file diff --git a/src/rove_zed/resource/rove_zed b/src/rove_zed/resource/rove_zed new file mode 100644 index 0000000..e69de29 diff --git a/src/rove_zed/setup.cfg b/src/rove_zed/setup.cfg new file mode 100644 index 0000000..36013e4 --- /dev/null +++ b/src/rove_zed/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rove_zed +[install] +install_scripts=$base/lib/rove_zed diff --git a/src/rove_zed/setup.py b/src/rove_zed/setup.py new file mode 100644 index 0000000..871e9e8 --- /dev/null +++ b/src/rove_zed/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'rove_zed' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', glob('launch/*.launch.py')), + ('share/' + package_name + '/config', glob('config/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='rove', + maintainer_email='capra@ens.etsmtl.ca', + description='Zed package for rove', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +)