diff --git a/ardupilot_gz_bringup/config/wildthumper_bridge.yaml b/ardupilot_gz_bringup/config/wildthumper_bridge.yaml index 2cc3d33..e505e16 100644 --- a/ardupilot_gz_bringup/config/wildthumper_bridge.yaml +++ b/ardupilot_gz_bringup/config/wildthumper_bridge.yaml @@ -48,3 +48,9 @@ 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/launch/robots/wildthumper.launch.py b/ardupilot_gz_bringup/launch/robots/wildthumper.launch.py index 6651f6c..a115847 100644 --- a/ardupilot_gz_bringup/launch/robots/wildthumper.launch.py +++ b/ardupilot_gz_bringup/launch/robots/wildthumper.launch.py @@ -127,7 +127,7 @@ def generate_launch_description(): # Load SDF file. sdf_file = os.path.join( - pkg_ardupilot_sitl_models, "models", "wildthumper", "model.sdf" + pkg_ardupilot_sitl_models, "models", "wildthumper_with_lidar", "model.sdf" ) with open(sdf_file, "r") as infp: robot_desc = infp.read() @@ -138,6 +138,10 @@ def generate_launch_description(): "model://wildthumper", "package://ardupilot_sitl_models/models/wildthumper") + robot_desc = robot_desc.replace( + "model://wildthumper_with_lidar", + "package://ardupilot_sitl_models/models/wildthumper_with_lidar") + # Publish /tf and /tf_static. robot_state_publisher = Node( package="robot_state_publisher", diff --git a/ardupilot_gz_bringup/launch/wildthumper_playpen.launch.py b/ardupilot_gz_bringup/launch/wildthumper_playpen.launch.py index f8cec89..00ac041 100644 --- a/ardupilot_gz_bringup/launch/wildthumper_playpen.launch.py +++ b/ardupilot_gz_bringup/launch/wildthumper_playpen.launch.py @@ -48,7 +48,6 @@ def generate_launch_description(): """Generate a launch description for a wildthumper rover.""" - pkg_ardupilot_sitl_models = get_package_share_directory("ardupilot_sitl_models") pkg_project_bringup = get_package_share_directory("ardupilot_gz_bringup") pkg_project_gazebo = get_package_share_directory("ardupilot_gz_gazebo") pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim") @@ -76,7 +75,7 @@ def generate_launch_description(): ), launch_arguments={ "gz_args": "-v4 -s -r " - + os.path.join(pkg_ardupilot_sitl_models, "worlds", "wildthumper_playpen.sdf") + + os.path.join(pkg_project_gazebo, "worlds", "wildthumper_playpen.sdf") }.items(), ) diff --git a/ardupilot_gz_bringup/rviz/wildthumper.rviz b/ardupilot_gz_bringup/rviz/wildthumper.rviz index c6033e7..51dbe7e 100644 --- a/ardupilot_gz_bringup/rviz/wildthumper.rviz +++ b/ardupilot_gz_bringup/rviz/wildthumper.rviz @@ -114,6 +114,11 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + lidar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true mid_left_motor_asm_link: Alpha: 1 Show Axes: false @@ -203,6 +208,8 @@ Visualization Manager: Value: true imu_link: Value: true + lidar_link: + Value: true mid_left_motor_asm_link: Value: true mid_left_wheel_link: @@ -248,6 +255,8 @@ Visualization Manager: {} imu_link: {} + lidar_link: + {} mid_wheel_chassis_long_link: mid_left_motor_asm_link: mid_left_wheel_link: @@ -307,6 +316,40 @@ Visualization Manager: Reliability Policy: Reliable Value: /odometry Value: true + - 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/LaserScan + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: "" + 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: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Name: WildThumper Enabled: true diff --git a/ardupilot_gz_gazebo/worlds/wildthumper_playpen.sdf b/ardupilot_gz_gazebo/worlds/wildthumper_playpen.sdf new file mode 100755 index 0000000..4c96240 --- /dev/null +++ b/ardupilot_gz_gazebo/worlds/wildthumper_playpen.sdf @@ -0,0 +1,73 @@ + + + + + 0.001 + 1.0 + + + + + + ogre2 + + + + + + + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + -35.3632621 + 149.1652374 + 10.0 + 0 + EARTH_WGS84 + + + + 0 0 0 0 0 0 + model://rover_playpen + + + + 0 0 0.15 0 0 0 + model://wildthumper_with_lidar + wildthumper + + + + diff --git a/ros2_gz.repos b/ros2_gz.repos index c3ce677..794707e 100644 --- a/ros2_gz.repos +++ b/ros2_gz.repos @@ -11,6 +11,10 @@ repositories: type: git url: https://github.com/ArduPilot/ardupilot_gz.git version: main + ardupilot_sitl_models: + type: git + url: https://github.com/ArduPilot/SITL_Models.git + version: main micro_ros_agent: type: git url: https://github.com/micro-ROS/micro-ROS-Agent.git diff --git a/ros2_gz_macos.repos b/ros2_gz_macos.repos index 64f3d31..a2560ae 100644 --- a/ros2_gz_macos.repos +++ b/ros2_gz_macos.repos @@ -11,6 +11,10 @@ repositories: type: git url: https://github.com/ArduPilot/ardupilot_gz.git version: main + ardupilot_sitl_models: + type: git + url: https://github.com/ArduPilot/SITL_Models.git + version: main gps_umd: type: git url: https://github.com/swri-robotics/gps_umd.git