diff --git a/.gitattributes b/.gitattributes
index 0faee1790..d41d25c52 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -4,5 +4,5 @@
*.bmp filter=lfs diff=lfs merge=lfs -text
*.svg filter=lfs diff=lfs merge=lfs -text
*.pdf filter=lfs diff=lfs merge=lfs -text
-
+*.pgm filter=lfs diff=lfs merge=lfs -text
. !text !filter !merge !diff
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 000000000..93c700e85
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+.vscode/
+.idea/
diff --git a/README.md b/README.md
index fe8e16168..cafa318f6 100644
--- a/README.md
+++ b/README.md
@@ -17,10 +17,10 @@ git clone https://github.com/memristor/mep3.git ./foxy_ws/src/mep3
## Editing Webots world
- Install [Webots R2021b](https://github.com/cyberbotics/webots/releases/download/R2021b/webots_2021b_amd64.deb)
-- Open [`webots_data/worlds/eurobot_2022.wbt`](./webots_data/worlds/eurobot_2022.wbt) in Webots
+- Open [`mep3_simulation/webots_data/worlds/eurobot_2022.wbt`](./mep3_simulation/webots_data/worlds/eurobot_2022.wbt) in Webots
- Stop simulation and set time to `00:00:00`
- Save changes
-- Commit all changes except for [`Viewpoint`](./webots_data/worlds/eurobot_2022.wbt#L5-L7)
+- Commit all changes except for [`Viewpoint`](./mep3_simulation/webots_data/worlds/eurobot_2022.wbt#L5-L7)
## ROS 2 setup
@@ -49,4 +49,11 @@ ros2 launch mep3_simulation robot_launch.py
```sh
source /opt/ros/foxy/local_setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard
+```
+
+### Navigation
+
+To launch simulation with `rviz` and `nav2` run
+```sh
+ros2 launch mep3_simulation robot_launch.py rviz:=true nav:=true
```
\ No newline at end of file
diff --git a/mep3_simulation/launch/robot_launch.py b/mep3_simulation/launch/robot_launch.py
index 785e9e756..6ce7cc32c 100644
--- a/mep3_simulation/launch/robot_launch.py
+++ b/mep3_simulation/launch/robot_launch.py
@@ -2,6 +2,9 @@
import pathlib
import launch
from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from webots_ros2_core.webots_launcher import WebotsLauncher
@@ -38,9 +41,11 @@ def generate_launch_description():
arguments=['joint_state_broadcaster'] + controller_manager_timeout,
)
- # The node which interacts with a robot in the Webots simulation is located in the `webots_ros2_driver` package under name `driver`.
+ # The node which interacts with a robot in the Webots simulation is located in the
+ # `webots_ros2_driver` package under name `driver`.
# It is necessary to run such a node for each robot in the simulation.
- # Typically, we provide it the `robot_description` parameters from a URDF file and `ros2_control_params` from the `ros2_control` configuration file.
+ # Typically, we provide it the `robot_description` parameters from a URDF file and
+ # `ros2_control_params` from the `ros2_control` configuration file.
webots_robot_driver = Node(
package='webots_ros2_driver',
executable='driver',
@@ -53,9 +58,13 @@ def generate_launch_description():
]
)
- # Often we want to publish robot transforms, so we use the `robot_state_publisher` node for that.
- # If robot model is not specified in the URDF file then Webots can help us with the URDF exportation feature.
- # Since the exportation feature is available only once the simulation has started and the `robot_state_publisher` node requires a `robot_description` parameter before we have to specify a dummy robot.
+ # Often we want to publish robot transforms, so we use the `robot_state_publisher`
+ # node for that.
+ # If robot model is not specified in the URDF file then Webots can help us with the
+ # URDF exportation feature.
+ # Since the exportation feature is available only once the simulation has started and
+ # the `robot_state_publisher` node requires a `robot_description` parameter before we
+ # have to specify a dummy robot.
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
@@ -65,21 +74,69 @@ def generate_launch_description():
}],
)
+ use_rviz = LaunchConfiguration('rviz', default=False)
+ use_nav = LaunchConfiguration('nav', default=False)
+ use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+ nav2_map = os.path.join(package_dir, 'resource', 'map.yml')
+
+ rviz_config = os.path.join(
+ get_package_share_directory('mep3_simulation'),
+ 'resource',
+ 'default.rviz'
+ )
+
+ rviz = Node(
+ package='rviz2',
+ executable='rviz2',
+ output='screen',
+ arguments=['--display-config=' + rviz_config],
+ parameters=[{'use_sim_time': use_sim_time}],
+ condition=launch.conditions.IfCondition(use_rviz)
+ )
+
+ nav2 = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(os.path.join(
+ get_package_share_directory('nav2_bringup'),
+ 'launch',
+ 'bringup_launch.py'
+ )),
+ launch_arguments=[
+ ('map', nav2_map),
+ ('use_sim_time', use_sim_time),
+ ],
+ condition=launch.conditions.IfCondition(use_nav)
+ )
+
+ map_odom_publisher = Node(
+ package='tf2_ros',
+ executable='static_transform_publisher',
+ output='screen',
+ arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
+ )
+
# Standard ROS 2 launch description
return launch.LaunchDescription([
+ # Wheel controller
joint_state_broadcaster_spawner,
diffdrive_controller_spawner,
# Start the Webots node
webots,
+ # 3D Visualization
+ rviz,
+
# Start the Webots robot driver
webots_robot_driver,
# Start the robot_state_publisher
robot_state_publisher,
+ # Navigation 2
+ nav2,
+ map_odom_publisher,
+
# This action will kill all nodes once the Webots simulation has exited
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
diff --git a/mep3_simulation/package.xml b/mep3_simulation/package.xml
index 49bed53ef..cda3e919f 100644
--- a/mep3_simulation/package.xml
+++ b/mep3_simulation/package.xml
@@ -13,6 +13,10 @@
ament_flake8
ament_pep257
python3-pytest
+ rviz2
+ tf2_ros
+ navigation2
+ nav2_bringup
ament_python
diff --git a/mep3_simulation/resource/default.rviz b/mep3_simulation/resource/default.rviz
new file mode 100644
index 000000000..634a90066
--- /dev/null
+++ b/mep3_simulation/resource/default.rviz
@@ -0,0 +1,757 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /Grid1
+ - /TF1/Frames1
+ - /TF1/Tree1
+ Splitter Ratio: 0.5833333134651184
+ Tree Height: 658
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: nav2_rviz_plugins/Navigation 2
+ Name: Navigation 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/RobotModel
+ Collision Enabled: false
+ Description File: ""
+ Description Source: Topic
+ Description Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /robot_description
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ Torso:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ accelerometer:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_cover_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ base_footprint:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ base_sonar_01_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_sonar_02_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_sonar_03_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ caster_back_left_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_back_left_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_back_right_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_back_right_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_front_left_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_front_left_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_front_right_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_front_right_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ display:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ gyro:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ head_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ hokuyo:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ inertial unit:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ torso_lift_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wheel_left_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wheel_right_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz_default_plugins/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ Torso:
+ Value: true
+ accelerometer:
+ Value: true
+ base_cover_link:
+ Value: true
+ base_footprint:
+ Value: true
+ base_link:
+ Value: true
+ base_sonar_01_link:
+ Value: true
+ base_sonar_02_link:
+ Value: true
+ base_sonar_03_link:
+ Value: true
+ camera:
+ Value: true
+ caster_back_left_1_link:
+ Value: true
+ caster_back_left_2_link:
+ Value: true
+ caster_back_right_1_link:
+ Value: true
+ caster_back_right_2_link:
+ Value: true
+ caster_front_left_1_link:
+ Value: true
+ caster_front_left_2_link:
+ Value: true
+ caster_front_right_1_link:
+ Value: true
+ caster_front_right_2_link:
+ Value: true
+ display:
+ Value: true
+ gyro:
+ Value: true
+ head_1_link:
+ Value: true
+ head_2_link:
+ Value: true
+ hokuyo:
+ Value: true
+ hokuyo_rotated:
+ Value: true
+ inertial unit:
+ Value: true
+ map:
+ Value: true
+ odom:
+ Value: true
+ torso_lift_link:
+ Value: true
+ wheel_left_link:
+ Value: true
+ wheel_right_link:
+ Value: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: false
+ Tree:
+ map:
+ odom:
+ base_link:
+ Torso:
+ torso_lift_link:
+ head_1_link:
+ head_2_link:
+ camera: {}
+ accelerometer: {}
+ base_cover_link: {}
+ base_footprint: {}
+ base_sonar_01_link: {}
+ base_sonar_02_link: {}
+ base_sonar_03_link: {}
+ caster_back_left_1_link:
+ caster_back_left_2_link: {}
+ caster_back_right_1_link:
+ caster_back_right_2_link: {}
+ caster_front_left_1_link:
+ caster_front_left_2_link: {}
+ caster_front_right_1_link:
+ caster_front_right_2_link: {}
+ display: {}
+ gyro: {}
+ hokuyo:
+ hokuyo_rotated: {}
+ inertial unit: {}
+ wheel_left_link: {}
+ wheel_right_link: {}
+ 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: 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
+ History Policy: Keep Last
+ Reliability Policy: Best Effort
+ Value: /scan
+ Use Fixed Frame: true
+ Use rainbow: true
+ 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/PointCloud2
+ 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: Bumper Hit
+ Position Transformer: ""
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.07999999821186066
+ Style: Spheres
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Best Effort
+ Value: /mobile_base/sensors/bumper_pointcloud
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/Map
+ Color Scheme: map
+ Draw Behind: true
+ Enabled: true
+ Name: Map
+ Topic:
+ Depth: 1
+ Durability Policy: Transient Local
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /map
+ Update Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /map_updates
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Arrow Length: 0.019999999552965164
+ Axes Length: 0.30000001192092896
+ Axes Radius: 0.009999999776482582
+ Class: rviz_default_plugins/PoseArray
+ Color: 0; 180; 0
+ Enabled: true
+ Head Length: 0.07000000029802322
+ Head Radius: 0.029999999329447746
+ Name: Amcl Particle Swarm
+ Shaft Length: 0.23000000417232513
+ Shaft Radius: 0.009999999776482582
+ Shape: Arrow (Flat)
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Best Effort
+ Value: /particlecloud
+ Value: true
+ - Class: rviz_common/Group
+ Displays:
+ - Alpha: 0.30000001192092896
+ Class: rviz_default_plugins/Map
+ Color Scheme: costmap
+ Draw Behind: false
+ Enabled: true
+ Name: Global Costmap
+ Topic:
+ Depth: 1
+ Durability Policy: Transient Local
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /global_costmap/costmap
+ Update Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /global_costmap/costmap_updates
+ Use Timestamp: false
+ Value: true
+ - Alpha: 0.30000001192092896
+ Class: rviz_default_plugins/Map
+ Color Scheme: costmap
+ Draw Behind: false
+ Enabled: true
+ Name: Downsampled Costmap
+ Topic:
+ Depth: 1
+ Durability Policy: Transient Local
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /downsampled_costmap
+ Update Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /downsampled_costmap_updates
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 255; 0; 0
+ Enabled: true
+ Head Diameter: 0.019999999552965164
+ Head Length: 0.019999999552965164
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: Arrows
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.004999999888241291
+ Shaft Length: 0.019999999552965164
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /plan
+ 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/PointCloud
+ Color: 125; 125; 125
+ 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: VoxelGrid
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.05000000074505806
+ Style: Boxes
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /global_costmap/voxel_marked_cloud
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/Polygon
+ Color: 25; 255; 0
+ Enabled: false
+ Name: Polygon
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /global_costmap/published_footprint
+ Value: false
+ Enabled: true
+ Name: Global Planner
+ - Class: rviz_common/Group
+ Displays:
+ - Alpha: 0.699999988079071
+ Class: rviz_default_plugins/Map
+ Color Scheme: costmap
+ Draw Behind: false
+ Enabled: true
+ Name: Local Costmap
+ Topic:
+ Depth: 1
+ Durability Policy: Transient Local
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /local_costmap/costmap
+ Update Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /local_costmap/costmap_updates
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 0; 12; 255
+ Enabled: true
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: Local Plan
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /local_plan
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: Trajectories
+ Namespaces: {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /marker
+ Value: false
+ - Alpha: 1
+ Class: rviz_default_plugins/Polygon
+ Color: 25; 255; 0
+ Enabled: true
+ Name: Polygon
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /local_costmap/published_footprint
+ 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/PointCloud
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ 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: VoxelGrid
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /local_costmap/voxel_marked_cloud
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ Enabled: true
+ Name: Controller
+ - Class: rviz_common/Group
+ Displays:
+ - Class: rviz_default_plugins/Image
+ Enabled: true
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: RealsenseCamera
+ Normalize Range: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /intel_realsense_r200_depth/image_raw
+ 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/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ 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: RealsenseDepthImage
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /intel_realsense_r200_depth/points
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ Enabled: false
+ Name: Realsense
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: MarkerArray
+ Namespaces: {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /waypoints
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ - Class: nav2_rviz_plugins/GoalTool
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 15.031473159790039
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: -5.242790699005127
+ Y: 0.4458843171596527
+ Z: 0.7334511876106262
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.7097968459129333
+ Target Frame:
+ Value: Orbit (rviz_default_plugins)
+ Yaw: 4.387388229370117
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: true
+ Height: 1016
+ Hide Left Dock: true
+ Hide Right Dock: true
+ Navigation 2:
+ collapsed: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a0000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002cf000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000312000000c9000000b700fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003c00000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ RealsenseCamera:
+ collapsed: false
+ Selection:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 960
+ X: 960
+ Y: 27
diff --git a/mep3_simulation/resource/map.pgm b/mep3_simulation/resource/map.pgm
new file mode 100644
index 000000000..f87676d66
--- /dev/null
+++ b/mep3_simulation/resource/map.pgm
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:63826289f0b86eabdfef736175900335115318c8ad19fac7b1668f92ad933d39
+size 960062
diff --git a/mep3_simulation/resource/map.yml b/mep3_simulation/resource/map.yml
new file mode 100644
index 000000000..3caf6f341
--- /dev/null
+++ b/mep3_simulation/resource/map.yml
@@ -0,0 +1,7 @@
+image: map.pgm
+mode: trinary
+resolution: 0.0025
+origin: [-1.5, -1, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
diff --git a/mep3_simulation/setup.py b/mep3_simulation/setup.py
index f555c564b..2bf16c7c8 100644
--- a/mep3_simulation/setup.py
+++ b/mep3_simulation/setup.py
@@ -9,6 +9,9 @@
'robot_launch.py'
],
'resource': [
+ 'default.rviz',
+ 'map.pgm',
+ 'map.yml',
'ros2_control_configuration.yml',
'webots_robot_description.urdf'
]