diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..c27778973 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "flatland"] + path = flatland + url = git@github.com:avidbots/flatland.git + branch = ros2-humble diff --git a/beluga_example/CMakeLists.txt b/beluga_example/CMakeLists.txt index 3e0b0059f..848e8deaf 100644 --- a/beluga_example/CMakeLists.txt +++ b/beluga_example/CMakeLists.txt @@ -7,7 +7,9 @@ find_package(beluga_amcl REQUIRED) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY models DESTINATION share/${PROJECT_NAME}) install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/beluga_example/launch/example_launch.py b/beluga_example/launch/example_launch.py index 6682dba93..2fd1304fb 100644 --- a/beluga_example/launch/example_launch.py +++ b/beluga_example/launch/example_launch.py @@ -74,6 +74,23 @@ def generate_launch_description(): {"node_names": ["map_server", "amcl"]}, ], ), + Node( + package="flatland_server", + executable="flatland_server", + output="screen", + parameters=[ + { + "world_path": os.path.join( + example_dir, "worlds", "turtlebot3_world.yaml" + ) + }, + {"update_rate": 200.0}, + {"step_size": 0.005}, + {"show_viz": False}, + {"viz_pub_rate": 30.0}, + {"use_sim_time": True}, + ], + ), ] ) diff --git a/beluga_example/models/turtlebot.model.yaml b/beluga_example/models/turtlebot.model.yaml new file mode 100644 index 000000000..f7dfaa88c --- /dev/null +++ b/beluga_example/models/turtlebot.model.yaml @@ -0,0 +1,50 @@ +bodies: + - name: base + pose: [0, 0, 0] + type: dynamic + color: [1, 1, 1, 0.75] + footprints: + - type: circle + radius: 0.5 + center: [-1, 0.0] + density: 1.0 + + - type: polygon + sensor: true + points: [[-.45, -.05], [-.45, 0.05], [-.35, 0.05], [-.35, -0.05]] + layers: [] + density: 1.0 + + - type: polygon + sensor: true + points: [[-.125, -.4], [-.125, -.3], [.125, -.3], [.125, -.4]] + density: 1.0 + + - type: polygon + sensor: true + points: [[-.125, .4], [-.125, .3], [.125, .3], [.125, .4]] + density: 100000 + +plugins: + - type: DiffDrive + name: turtlebot_drive + body: base + twist_sub: cmd_vel + odom_frame_id: map + pub_rate: 10 + + - type: ModelTfPublisher + name: tf_publisher + publish_tf_world: true + + - type: Laser + name: laser_front + frame: laser_front + topic: scan + body: base + broadcast_tf: true + origin: [0.28, 0, 0] + range: 20 + angle: {min: -2.356194490192345, max: 2.356194490192345, increment: 0.004363323129985824} + noise_std_dev: 0.02 + update_rate: 40 diff --git a/beluga_example/package.xml b/beluga_example/package.xml index 579d9feaa..c276f87fa 100644 --- a/beluga_example/package.xml +++ b/beluga_example/package.xml @@ -13,8 +13,9 @@ ament_cmake launch_ros - launch_ros beluga_amcl + flatland_server + launch_ros nav2_lifecycle_manager nav2_map_server nav2_rviz_plugins diff --git a/beluga_example/rviz/rviz.rviz b/beluga_example/rviz/rviz.rviz index 56b717b38..c45814ef5 100644 --- a/beluga_example/rviz/rviz.rviz +++ b/beluga_example/rviz/rviz.rviz @@ -9,8 +9,9 @@ Panels: - /ParticleCloud1 - /Map1 - /TF1 + - /LaserScan1 Splitter Ratio: 0.5 - Tree Height: 1555 + Tree Height: 1166 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -28,7 +29,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: LaserScan Visualization Manager: Class: "" Displays: @@ -54,8 +55,8 @@ Visualization Manager: Class: nav2_rviz_plugins/ParticleCloud Color: 255; 25; 0 Enabled: true - Max Arrow Length: 0.1 - Min Arrow Length: 0.05 + Max Arrow Length: 0.10000000149011612 + Min Arrow Length: 0.05000000074505806 Name: ParticleCloud Shape: Arrow (3D) Topic: @@ -92,15 +93,58 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true + base: + Value: true + laser_front: + Value: true + map: + Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - {} + map: + base: + laser_front: + {} Update Interval: 0 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: 0; 255; 0 + Color Transformer: FlatColor + 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: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Spheres + 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 Global Options: Background Color: 48; 48; 48 @@ -147,7 +191,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 16.030778884887695 + Distance: 7.173450469970703 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -162,18 +206,18 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.9253982305526733 + Pitch: 1.100398302078247 Target Frame: Value: Orbit (rviz) - Yaw: 2.375394582748413 + Yaw: 3.5103964805603027 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 2032 + Height: 1536 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000701fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000007010000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f00000701fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000007010000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7000000051fc0100000002fb0000000800540069006d0065010000000000000e700000045300fffffffb0000000800540069006d0065010000000000000450000000000000000000000b2d0000070100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000553fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000553000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f00000553fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000553000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b800000051fc0100000002fb0000000800540069006d00650100000000000009b80000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000006810000055300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -182,6 +226,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 3696 - X: 144 - Y: 54 + Width: 2488 + X: 72 + Y: 27 diff --git a/beluga_example/worlds/turtlebot3_world.yaml b/beluga_example/worlds/turtlebot3_world.yaml new file mode 100644 index 000000000..5e837e3fb --- /dev/null +++ b/beluga_example/worlds/turtlebot3_world.yaml @@ -0,0 +1,9 @@ +properties: {} + +layers: + - name: "map" + map: "../maps/turtlebot3_world.yaml" +models: + - name: turtlebot + model: "../models/turtlebot.model.yaml" + pose: [0, 2, 0] diff --git a/flatland b/flatland new file mode 160000 index 000000000..c6adc83b1 --- /dev/null +++ b/flatland @@ -0,0 +1 @@ +Subproject commit c6adc83b1dee57e1ba1750359f763ec690540df2