diff --git a/ardupilot_gz_bringup/launch/robots/iris.launch.py b/ardupilot_gz_bringup/launch/robots/iris.launch.py
index 4e39dcc..e1f9d6d 100644
--- a/ardupilot_gz_bringup/launch/robots/iris.launch.py
+++ b/ardupilot_gz_bringup/launch/robots/iris.launch.py
@@ -40,14 +40,23 @@
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.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_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
+
def generate_launch_description():
"""Generate a launch description for a iris quadcopter."""
pkg_ardupilot_sitl = get_package_share_directory("ardupilot_sitl")
@@ -122,7 +131,7 @@ def generate_launch_description():
robot_desc = infp.read()
# print(robot_desc)
- # Remap the /tf and /tf_static under /ignore as the TF conflicts.
+ # Publish /tf and /tf_static.
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
@@ -130,6 +139,7 @@ def generate_launch_description():
output="both",
parameters=[
{"robot_description": robot_desc},
+ {"frame_prefix": ""},
],
)
@@ -148,10 +158,52 @@ 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",
+ executable="relay",
+ arguments=[
+ "/gz/tf",
+ "/tf",
+ ],
+ output="screen",
+ respawn=False,
+ 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(
+ OnProcessStart(
+ target_action=bridge,
+ on_start=[
+ topic_tools_tf
+ ]
+ )
+ ),
]
)
diff --git a/ardupilot_gz_bringup/launch/robots/iris_lidar.launch.py b/ardupilot_gz_bringup/launch/robots/iris_lidar.launch.py
index 6bcab33..19892f0 100644
--- a/ardupilot_gz_bringup/launch/robots/iris_lidar.launch.py
+++ b/ardupilot_gz_bringup/launch/robots/iris_lidar.launch.py
@@ -40,8 +40,16 @@
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.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_ros.actions import Node
@@ -125,7 +133,7 @@ def generate_launch_description():
robot_desc = infp.read()
# print(robot_desc)
- # Remap the /tf and /tf_static under /ignore as the TF conflicts.
+ # Publish /tf and /tf_static.
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
@@ -133,6 +141,7 @@ def generate_launch_description():
output="both",
parameters=[
{"robot_description": robot_desc},
+ {"frame_prefix": ""},
],
)
@@ -151,10 +160,52 @@ 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",
+ executable="relay",
+ arguments=[
+ "/gz/tf",
+ "/tf",
+ ],
+ output="screen",
+ respawn=False,
+ 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(
+ OnProcessStart(
+ target_action=bridge,
+ on_start=[
+ topic_tools_tf
+ ]
+ )
+ ),
]
)
diff --git a/ardupilot_gz_bringup/package.xml b/ardupilot_gz_bringup/package.xml
index d874297..fdc2f96 100644
--- a/ardupilot_gz_bringup/package.xml
+++ b/ardupilot_gz_bringup/package.xml
@@ -16,6 +16,7 @@
launch_ros
ros_gz_sim
robot_state_publisher
+ topic_tools
ament_cmake_pytest
ament_lint_auto
ament_lint_common
diff --git a/ardupilot_gz_bringup/rviz/iris.rviz b/ardupilot_gz_bringup/rviz/iris.rviz
index 98d97be..3964d27 100644
--- a/ardupilot_gz_bringup/rviz/iris.rviz
+++ b/ardupilot_gz_bringup/rviz/iris.rviz
@@ -4,8 +4,8 @@ Panels:
Name: Displays
Property Tree Widget:
Expanded:
+ - /Global Options1
- /Iris1
- - /Iris1/TF1
Splitter Ratio: 0.5
Tree Height: 288
- Class: rviz_common/Selection
@@ -122,7 +122,7 @@ Visualization Manager:
Inertia: false
Mass: false
Name: RobotModel
- TF Prefix: iris
+ TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
@@ -131,27 +131,25 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: true
- iris:
- Value: true
- iris/base_link:
+ base_link:
Value: true
- iris/gimbal_link:
+ gimbal_link:
Value: true
- iris/imu_link:
+ imu_link:
Value: true
- iris/odom:
+ odom:
Value: true
- iris/roll_link:
+ roll_link:
Value: true
- iris/rotor_0:
+ rotor_0:
Value: true
- iris/rotor_1:
+ rotor_1:
Value: true
- iris/rotor_2:
+ rotor_2:
Value: true
- iris/rotor_3:
+ rotor_3:
Value: true
- iris/tilt_link:
+ tilt_link:
Value: true
Marker Scale: 1
Name: TF
@@ -159,26 +157,23 @@ Visualization Manager:
Show Axes: true
Show Names: false
Tree:
- iris/odom:
- iris:
- iris/base_link:
- {}
- iris/gimbal_link:
- {}
- iris/imu_link:
- {}
- iris/roll_link:
- {}
- iris/rotor_0:
- {}
- iris/rotor_1:
- {}
- iris/rotor_2:
- {}
- iris/rotor_3:
- {}
- iris/tilt_link:
- {}
+ base_link:
+ gimbal_link:
+ roll_link:
+ tilt_link:
+ {}
+ imu_link:
+ {}
+ rotor_0:
+ {}
+ rotor_1:
+ {}
+ rotor_2:
+ {}
+ rotor_3:
+ {}
+ odom:
+ {}
Update Interval: 0
Value: true
- Angle Tolerance: 0.10000000149011612
@@ -239,7 +234,7 @@ Visualization Manager:
Enabled: true
Global Options:
Background Color: 48; 48; 48
- Fixed Frame: iris/odom
+ Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
@@ -321,4 +316,4 @@ Window Geometry:
collapsed: false
Width: 1200
X: 0
- Y: 25
+ Y: 38
diff --git a/ardupilot_gz_bringup/rviz/iris_with_lidar.rviz b/ardupilot_gz_bringup/rviz/iris_with_lidar.rviz
index fb5a280..830b666 100644
--- a/ardupilot_gz_bringup/rviz/iris_with_lidar.rviz
+++ b/ardupilot_gz_bringup/rviz/iris_with_lidar.rviz
@@ -8,7 +8,7 @@ Panels:
- /Status1
- /Iris1
Splitter Ratio: 0.5
- Tree Height: 581
+ Tree Height: 587
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -26,7 +26,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: ""
+ SyncSource: LaserScan
Visualization Manager:
Class: ""
Displays:
@@ -113,7 +113,7 @@ Visualization Manager:
Inertia: false
Mass: false
Name: RobotModel
- TF Prefix: iris
+ TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
@@ -122,23 +122,21 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: true
- iris:
- Value: true
- iris/base_link:
+ base_link:
Value: true
- iris/base_scan:
+ base_scan:
Value: true
- iris/imu_link:
+ imu_link:
Value: true
- iris/odom:
+ odom:
Value: true
- iris/rotor_0:
+ rotor_0:
Value: true
- iris/rotor_1:
+ rotor_1:
Value: true
- iris/rotor_2:
+ rotor_2:
Value: true
- iris/rotor_3:
+ rotor_3:
Value: true
Marker Scale: 1
Name: TF
@@ -146,21 +144,19 @@ Visualization Manager:
Show Axes: true
Show Names: false
Tree:
- iris/odom:
- iris:
- iris/base_link:
+ odom:
+ base_link:
+ base_scan:
{}
- iris/base_scan:
+ imu_link:
{}
- iris/imu_link:
+ rotor_0:
{}
- iris/rotor_0:
+ rotor_1:
{}
- iris/rotor_1:
+ rotor_2:
{}
- iris/rotor_2:
- {}
- iris/rotor_3:
+ rotor_3:
{}
Update Interval: 0
Value: true
@@ -203,12 +199,46 @@ 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: 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: LaserScan
+ 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: /scan
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
Enabled: true
Name: Iris
Enabled: true
Global Options:
Background Color: 48; 48; 48
- Fixed Frame: iris/odom
+ Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
@@ -251,16 +281,16 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
- Distance: 10.05544662475586
+ Distance: 9.583603858947754
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 0.4391220510005951
- Y: 1.8562581539154053
- Z: -0.781160831451416
+ X: 0.46440741419792175
+ Y: 2.6354382038116455
+ Z: 0.06404712796211243
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
@@ -269,7 +299,7 @@ Visualization Manager:
Pitch: 0.6897952556610107
Target Frame:
Value: Orbit (rviz)
- Yaw: 5.143586158752441
+ Yaw: 5.103586196899414
Saved: ~
Window Geometry:
Displays:
@@ -277,7 +307,7 @@ Window Geometry:
Height: 800
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000016300000282fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000282000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001a20000011d0000000000000000000000010000010f0000029bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c0000029b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004af0000003efc0100000002fb0000000800540069006d00650100000000000004af000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003460000028200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000400000000000001630000029afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c0000029a000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001a20000011d0000000000000000000000010000010f0000029bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c0000029b000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004af0000003efc0100000002fb0000000800540069006d00650100000000000004af0000023d00fffffffb0000000800540069006d006501000000000000045000000000000000000000034b0000029a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -286,6 +316,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
- Width: 1199
- X: 70
- Y: 27
+ Width: 1200
+ X: 0
+ Y: 38
diff --git a/ardupilot_gz_description/models/iris_with_lidar/model.sdf b/ardupilot_gz_description/models/iris_with_lidar/model.sdf
index 63c1366..7c78c1c 100755
--- a/ardupilot_gz_description/models/iris_with_lidar/model.sdf
+++ b/ardupilot_gz_description/models/iris_with_lidar/model.sdf
@@ -10,20 +10,11 @@
name="gz::sim::systems::JointStatePublisher">
-
- true
- true
- true
- 1
-
-
- iris/odom
- iris
+ odom
+ base_link
3
diff --git a/ardupilot_gz_gazebo/worlds/iris_maze.sdf b/ardupilot_gz_gazebo/worlds/iris_maze.sdf
index 9b2801f..53a0107 100755
--- a/ardupilot_gz_gazebo/worlds/iris_maze.sdf
+++ b/ardupilot_gz_gazebo/worlds/iris_maze.sdf
@@ -429,7 +429,7 @@
model://iris_with_lidar
iris
- 0 0 0.194923 0 0 90
+ 0 0 0.194923 0 0 0