From 37bdd683a0e324d8e0c27e811675c442c28d6013 Mon Sep 17 00:00:00 2001 From: Tejal Ashwini Barnwal <64950661+tejalbarnwal@users.noreply.github.com> Date: Fri, 31 Jan 2025 22:12:22 +0530 Subject: [PATCH] Add 2D/3D Lidar Launch Arg in `iris_with_lidar.launch.py` (#64) * add models for iris with 2d and 3d lidar * add 2D/3D lidar launch arg and point cloud ros_gz bridge config * remove iris_with_lidar model * add dynamic lidar model selection and use ros_gz_sim to spawn robot in the world * remove overriding of lidar_dim argument * use separate config file for lidar config instead of loading both in ros_gz_bridge * add pointcloud representation in rviz --- ...r_bridge.yaml => iris_2Dlidar_bridge.yaml} | 12 +- .../config/iris_3Dlidar_bridge.yaml | 44 +++ .../launch/iris_maze.launch.py | 11 +- .../launch/robots/iris_lidar.launch.py | 294 ++++++++++++------ .../rviz/iris_with_lidar.rviz | 34 ++ .../models/iris_with_lidar/model.config | 2 +- .../models/iris_with_lidar/model.sdf | 61 +--- .../models/lidar_2d/model.config | 18 ++ .../models/lidar_2d/model.sdf | 64 ++++ .../models/lidar_3d/model.config | 18 ++ .../models/lidar_3d/model.sdf | 65 ++++ ardupilot_gz_gazebo/worlds/iris_maze.sdf | 7 - 12 files changed, 459 insertions(+), 171 deletions(-) rename ardupilot_gz_bringup/config/{iris_lidar_bridge.yaml => iris_2Dlidar_bridge.yaml} (100%) create mode 100644 ardupilot_gz_bringup/config/iris_3Dlidar_bridge.yaml create mode 100755 ardupilot_gz_description/models/lidar_2d/model.config create mode 100755 ardupilot_gz_description/models/lidar_2d/model.sdf create mode 100755 ardupilot_gz_description/models/lidar_3d/model.config create mode 100755 ardupilot_gz_description/models/lidar_3d/model.sdf diff --git a/ardupilot_gz_bringup/config/iris_lidar_bridge.yaml b/ardupilot_gz_bringup/config/iris_2Dlidar_bridge.yaml similarity index 100% rename from ardupilot_gz_bringup/config/iris_lidar_bridge.yaml rename to ardupilot_gz_bringup/config/iris_2Dlidar_bridge.yaml index 237be93..1e80b4d 100644 --- a/ardupilot_gz_bringup/config/iris_lidar_bridge.yaml +++ b/ardupilot_gz_bringup/config/iris_2Dlidar_bridge.yaml @@ -31,14 +31,14 @@ gz_type_name: "gz.msgs.IMU" direction: GZ_TO_ROS -- ros_topic_name: "scan" - gz_topic_name: "/lidar" - ros_type_name: "sensor_msgs/msg/LaserScan" - gz_type_name: "gz.msgs.LaserScan" - direction: GZ_TO_ROS - - ros_topic_name: "battery" gz_topic_name: "/model/iris/battery/linear_battery/state" ros_type_name: "sensor_msgs/msg/BatteryState" gz_type_name: "gz.msgs.BatteryState" direction: GZ_TO_ROS + +- ros_topic_name: "scan" + gz_topic_name: "/lidar" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS diff --git a/ardupilot_gz_bringup/config/iris_3Dlidar_bridge.yaml b/ardupilot_gz_bringup/config/iris_3Dlidar_bridge.yaml new file mode 100644 index 0000000..7ef4880 --- /dev/null +++ b/ardupilot_gz_bringup/config/iris_3Dlidar_bridge.yaml @@ -0,0 +1,44 @@ +--- +- ros_topic_name: "clock" + gz_topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS +- ros_topic_name: "joint_states" + gz_topic_name: "/world/map/model/iris/joint_state" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS +- ros_topic_name: "odometry" + gz_topic_name: "/model/iris/odometry" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + direction: GZ_TO_ROS +- ros_topic_name: "gz/tf" + gz_topic_name: "/model/iris/pose" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS +- ros_topic_name: "gz/tf_static" + gz_topic_name: "/model/iris/pose_static" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS + +- ros_topic_name: "imu" + gz_topic_name: "/world/map/model/iris/link/imu_link/sensor/imu_sensor/imu" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + direction: GZ_TO_ROS + +- ros_topic_name: "battery" + gz_topic_name: "/model/iris/battery/linear_battery/state" + ros_type_name: "sensor_msgs/msg/BatteryState" + gz_type_name: "gz.msgs.BatteryState" + direction: GZ_TO_ROS + +- ros_topic_name: "cloud_in" + gz_topic_name: "/lidar/points" + ros_type_name: "sensor_msgs/msg/PointCloud2" + gz_type_name: "gz.msgs.PointCloudPacked" + direction: GZ_TO_ROS \ No newline at end of file diff --git a/ardupilot_gz_bringup/launch/iris_maze.launch.py b/ardupilot_gz_bringup/launch/iris_maze.launch.py index 6e5326a..399e62a 100644 --- a/ardupilot_gz_bringup/launch/iris_maze.launch.py +++ b/ardupilot_gz_bringup/launch/iris_maze.launch.py @@ -65,7 +65,16 @@ def generate_launch_description(): ] ), ] - ) + ), + launch_arguments={ + "model": "iris_with_lidar", + "name": "iris", + "x": "0", + "y": "0", + "z": "0.194923", + "R": "0.0", + "P": "0.0", + }.items(), ) # Gazebo. diff --git a/ardupilot_gz_bringup/launch/robots/iris_lidar.launch.py b/ardupilot_gz_bringup/launch/robots/iris_lidar.launch.py index 4fb584a..746ee8e 100644 --- a/ardupilot_gz_bringup/launch/robots/iris_lidar.launch.py +++ b/ardupilot_gz_bringup/launch/robots/iris_lidar.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 ArduPilot.org. +# Copyright 2024 ArduPilot.org. # # This program is free software: you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by @@ -38,92 +38,88 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.actions import RegisterEventHandler - +from launch.actions import DeclareLaunchArgument, OpaqueFunction, LogInfo, IncludeLaunchDescription, RegisterEventHandler from launch.conditions import IfCondition - from launch.event_handlers import OnProcessStart - from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PathJoinSubstitution - +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +def launch_spawn_robot(context): + """Return a Gazebo spawn robot launch description""" + # Get substitutions for arguments + name = LaunchConfiguration("name") + pos_x = LaunchConfiguration("x") + pos_y = LaunchConfiguration("y") + pos_z = LaunchConfiguration("z") + rot_r = LaunchConfiguration("R") + rot_p = LaunchConfiguration("P") + rot_y = LaunchConfiguration("Y") -def generate_launch_description(): - """Generate a launch description for a iris quadcopter.""" - pkg_ardupilot_sitl = get_package_share_directory("ardupilot_sitl") - pkg_ardupilot_gz_description = get_package_share_directory( - "ardupilot_gz_description" - ) - pkg_project_bringup = get_package_share_directory("ardupilot_gz_bringup") - - # Include component launch files. - sitl_dds = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [ - FindPackageShare("ardupilot_sitl"), - "launch", - "sitl_dds_udp.launch.py", - ] - ), - ] - ), - launch_arguments={ - "transport": "udp4", - "port": "2019", - "synthetic_clock": "True", - "wipe": "False", - "model": "json", - "speedup": "1", - "slave": "0", - "instance": "0", - "defaults": os.path.join( - pkg_ardupilot_sitl, - "config", - "default_params", - "gazebo-iris.parm", - ) - + "," - + os.path.join( - pkg_ardupilot_sitl, - "config", - "default_params", - "dds_udp.parm", - ), - "sim_address": "127.0.0.1", - "master": "tcp:127.0.0.1:5760", - "sitl": "127.0.0.1:5501", - }.items(), + # spawn robot + spawn_robot = Node( + package="ros_gz_sim", + executable="create", + namespace=name, + arguments=[ + "-world", + "", + "-param", + "", + "-name", + name, + "-topic", + "/robot_description", + "-x", + pos_x, + "-y", + pos_y, + "-z", + pos_z, + "-R", + rot_r, + "-P", + rot_p, + "-Y", + rot_y, + ], + output="screen", ) + return [spawn_robot] - # Robot description. - # Ensure `SDF_PATH` is populated as `sdformat_urdf`` uses this rather - # than `GZ_SIM_RESOURCE_PATH` to locate resources. - if "GZ_SIM_RESOURCE_PATH" in os.environ: - gz_sim_resource_path = os.environ["GZ_SIM_RESOURCE_PATH"] - - if "SDF_PATH" in os.environ: - sdf_path = os.environ["SDF_PATH"] - os.environ["SDF_PATH"] = sdf_path + ":" + gz_sim_resource_path - else: - os.environ["SDF_PATH"] = gz_sim_resource_path - - # Load SDF file. +def launch_state_pub_with_bridge(context): + # Robot description and ros_gz bridge config chosen based on passed lidar_dimension argument + lidar_dim = LaunchConfiguration("lidar_dim").perform(context) + pkg_ardupilot_gz_description = get_package_share_directory("ardupilot_gz_description") + pkg_project_bringup = get_package_share_directory("ardupilot_gz_bringup") + sdf_file = os.path.join( pkg_ardupilot_gz_description, "models", "iris_with_lidar", "model.sdf" ) + with open(sdf_file, "r") as infp: robot_desc = infp.read() # print(robot_desc) + ros_gz_bridge_config = "iris_3Dlidar_bridge.yaml" + + # Load SDF file and choose ros_gz bridge config based on lidar dimensions + if lidar_dim == "2": + log = LogInfo(msg="Using iris_with_2d_lidar_model ") + robot_desc = robot_desc.replace("model://lidar_2d", "model://lidar_2d") + ros_gz_bridge_config = "iris_2Dlidar_bridge.yaml" + elif lidar_dim == "3": + log = LogInfo(msg="Using iris_with_3d_lidar_model") + robot_desc = robot_desc.replace("model://lidar_2d", "model://lidar_3d") + ros_gz_bridge_config = "iris_3Dlidar_bridge.yaml" + else: + log = LogInfo(msg="ERROR: unknown lidar dimensions! Defaulting to 3d lidar") + robot_desc = robot_desc.replace("model://lidar_2d", "model://lidar_3d") + ros_gz_bridge_config = "iris_3Dlidar_bridge.yaml" + + # Publish /tf and /tf_static. robot_state_publisher = Node( package="robot_state_publisher", @@ -136,14 +132,14 @@ def generate_launch_description(): ], ) - # Bridge. + # Bridge bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", parameters=[ { "config_file": os.path.join( - pkg_project_bringup, "config", "iris_lidar_bridge.yaml" + pkg_project_bringup, "config", ros_gz_bridge_config ), "qos_overrides./tf_static.publisher.durability": "transient_local", } @@ -151,24 +147,6 @@ def generate_launch_description(): output="screen", ) - # Transform - use if the model includes "gz::sim::systems::PosePublisher" - # and a filter is required. - # topic_tools_tf = Node( - # package="topic_tools", - # executable="transform", - # arguments=[ - # "/gz/tf", - # "/tf", - # "tf2_msgs/msg/TFMessage", - # "tf2_msgs.msg.TFMessage(transforms=[x for x in m.transforms if x.header.frame_id == 'odom'])", - # "--import", - # "tf2_msgs", - # "geometry_msgs", - # ], - # output="screen", - # respawn=True, - # ) - # Relay - use instead of transform when Gazebo is only publishing odom -> base_link topic_tools_tf = Node( package="topic_tools", @@ -182,21 +160,137 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration("use_gz_tf")), ) - return LaunchDescription( - [ - DeclareLaunchArgument( - "use_gz_tf", default_value="true", description="Use Gazebo TF." - ), - sitl_dds, - robot_state_publisher, - bridge, - RegisterEventHandler( + event = RegisterEventHandler( OnProcessStart( target_action=bridge, on_start=[ topic_tools_tf ] ) + ) + + return [log, robot_state_publisher, bridge, event] + +def generate_launch_arguments(): + """Generate a list of launch arguments""" + return [ + DeclareLaunchArgument( + "use_gz_tf", + default_value="true", + description="Use Gazebo TF." + ), + DeclareLaunchArgument( + "lidar_dim", + default_value="3", + description="Whether to use a 2D or 3D lidar" ), - ] + # Gazebo model launch arguments. + DeclareLaunchArgument( + "model", + default_value="iris_with_lidar", + description="Name or filepath of the model to load.", + ), + DeclareLaunchArgument( + "name", + default_value="iris", + description="Name for the model instance.", + ), + DeclareLaunchArgument( + "x", + default_value="0", + description="The intial 'x' position (m).", + ), + DeclareLaunchArgument( + "y", + default_value="0", + description="The intial 'y' position (m).", + ), + DeclareLaunchArgument( + "z", + default_value="0", + description="The intial 'z' position (m).", + ), + DeclareLaunchArgument( + "R", + default_value="0", + description="The intial roll angle (radians).", + ), + DeclareLaunchArgument( + "P", + default_value="0", + description="The intial pitch angle (radians).", + ), + DeclareLaunchArgument( + "Y", + default_value="0", + description="The intial yaw angle (radians).", + ), + ] + +def generate_launch_description(): + """Generate a launch description for a iris quadrotor""" + + launch_arguments = generate_launch_arguments() + + pkg_ardupilot_sitl = get_package_share_directory("ardupilot_sitl") + + # Include component launch files. + sitl_dds = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare("ardupilot_sitl"), + "launch", + "sitl_dds_udp.launch.py", + ] + ), + ] + ), + launch_arguments={ + "transport": "udp4", + "port": "2019", + "synthetic_clock": "True", + "wipe": "False", + "model": "json", + "speedup": "1", + "slave": "0", + "instance": "0", + "defaults": os.path.join( + pkg_ardupilot_sitl, + "config", + "default_params", + "gazebo-iris.parm", + ) + + "," + + os.path.join( + pkg_ardupilot_sitl, + "config", + "default_params", + "dds_udp.parm", + ), + "sim_address": "127.0.0.1", + "master": "tcp:127.0.0.1:5760", + "sitl": "127.0.0.1:5501", + }.items(), ) + + # Ensure `SDF_PATH` is populated as `sdformat_urdf`` uses this rather + # than `GZ_SIM_RESOURCE_PATH` to locate resources. + if "GZ_SIM_RESOURCE_PATH" in os.environ: + gz_sim_resource_path = os.environ["GZ_SIM_RESOURCE_PATH"] + + if "SDF_PATH" in os.environ: + sdf_path = os.environ["SDF_PATH"] + os.environ["SDF_PATH"] = sdf_path + ":" + gz_sim_resource_path + else: + os.environ["SDF_PATH"] = gz_sim_resource_path + + opfunc_robot_state_publisher = OpaqueFunction(function=launch_state_pub_with_bridge) + opfunc_spawn_robot = OpaqueFunction(function=launch_spawn_robot) + ld = LaunchDescription(launch_arguments) + ld.add_action(sitl_dds) + ld.add_action(opfunc_robot_state_publisher) + ld.add_action(opfunc_spawn_robot) + + return ld diff --git a/ardupilot_gz_bringup/rviz/iris_with_lidar.rviz b/ardupilot_gz_bringup/rviz/iris_with_lidar.rviz index 830b666..ac2b1ac 100644 --- a/ardupilot_gz_bringup/rviz/iris_with_lidar.rviz +++ b/ardupilot_gz_bringup/rviz/iris_with_lidar.rviz @@ -235,6 +235,40 @@ Visualization Manager: Value: true Enabled: true Name: Iris + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_in + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/ardupilot_gz_description/models/iris_with_lidar/model.config b/ardupilot_gz_description/models/iris_with_lidar/model.config index b1bdbca..34234b7 100755 --- a/ardupilot_gz_description/models/iris_with_lidar/model.config +++ b/ardupilot_gz_description/models/iris_with_lidar/model.config @@ -14,7 +14,7 @@ Starting with iris_with_ardupilot - Add lidar plugin, link and joint + Add 3D lidar plugin, link and joint diff --git a/ardupilot_gz_description/models/iris_with_lidar/model.sdf b/ardupilot_gz_description/models/iris_with_lidar/model.sdf index b7776a2..7f1c0b8 100755 --- a/ardupilot_gz_description/models/iris_with_lidar/model.sdf +++ b/ardupilot_gz_description/models/iris_with_lidar/model.sdf @@ -283,62 +283,11 @@ - - 0 0 0.075077 0 0 0 - - 0.1 - - 0.000166667 - 0.000166667 - 0.000166667 - - - - - - - 0.05 0.05 0.05 - - - - - - - - 0.05 0.05 0.05 - - - - - - base_scan - 0 0 0 0 0 0 - lidar - 10 - - - - 640 - 1 - -3.14159265 - 3.14159265 - - - 1 - 1 - 0.0 - 0.0 - - - - 0.08 - 10.0 - 0.01 - - - true - - + + model://lidar_2d + base_scan + 0 0 0.075077 0 0 0 + base_link diff --git a/ardupilot_gz_description/models/lidar_2d/model.config b/ardupilot_gz_description/models/lidar_2d/model.config new file mode 100755 index 0000000..34ca57f --- /dev/null +++ b/ardupilot_gz_description/models/lidar_2d/model.config @@ -0,0 +1,18 @@ + + + + Lidar 2D + 2.0 + model.sdf + + + Tejal Barnwal + tejalbarnwal@gmail.com + + + Tejal Barnwal + + + 2D lidar plugin and link on drone + + diff --git a/ardupilot_gz_description/models/lidar_2d/model.sdf b/ardupilot_gz_description/models/lidar_2d/model.sdf new file mode 100755 index 0000000..941009b --- /dev/null +++ b/ardupilot_gz_description/models/lidar_2d/model.sdf @@ -0,0 +1,64 @@ + + + + + + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + + 0.05 0.05 0.05 + + + + + + + + 0.05 0.05 0.05 + + + + + + base_scan + 0 0 0 0 0 0 + lidar + 10 + + + + 640 + 1 + -3.14159265 + 3.14159265 + + + 1 + 1 + 0.0 + 0.0 + + + + 0.08 + 10.0 + 0.01 + + + true + + + + + + diff --git a/ardupilot_gz_description/models/lidar_3d/model.config b/ardupilot_gz_description/models/lidar_3d/model.config new file mode 100755 index 0000000..e8cef30 --- /dev/null +++ b/ardupilot_gz_description/models/lidar_3d/model.config @@ -0,0 +1,18 @@ + + + + Lidar 3D + 2.0 + model.sdf + + + Tejal Barnwal + tejalbarnwal@gmail.com + + + Tejal Barnwal + + + 3D lidar plugin and link on drone + + diff --git a/ardupilot_gz_description/models/lidar_3d/model.sdf b/ardupilot_gz_description/models/lidar_3d/model.sdf new file mode 100755 index 0000000..9ed9876 --- /dev/null +++ b/ardupilot_gz_description/models/lidar_3d/model.sdf @@ -0,0 +1,65 @@ + + + + + + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + + 0.05 0.05 0.05 + + + + + + + + 0.05 0.05 0.05 + + + + + + base_scan + 0 0 0 0 0 0 + lidar + 15 + 0 + + + + 360 + 1 + -3.14159265 + 3.14159265 + + + 60 + 1 + -0.523599 + 0.523599 + + + + 0.5 + 10 + 0.05 + + + true + + + + + + diff --git a/ardupilot_gz_gazebo/worlds/iris_maze.sdf b/ardupilot_gz_gazebo/worlds/iris_maze.sdf index 0e5d16f..6ff82a7 100755 --- a/ardupilot_gz_gazebo/worlds/iris_maze.sdf +++ b/ardupilot_gz_gazebo/worlds/iris_maze.sdf @@ -425,12 +425,5 @@ - - - model://iris_with_lidar - iris - 0 0 0.195 0 0 0 - -