diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index 3c2b9a9cc..440fde748 100644 --- a/webots_ros2/CHANGELOG.rst +++ b/webots_ros2/CHANGELOG.rst @@ -4,8 +4,9 @@ Changelog for package webots_ros2 2023.0.4 (2023-XX-XX) ------------------ -* Added support for painted point clouds +* Added support for painted point clouds. * Fixed ability to launch RViz without other tools in e-puck example. +* Added new TIAGo project to webots_ros2_tiago to run real robot configuration. 2023.0.3 (2023-04-12) ------------------ diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst index b4057c500..96ba60995 100644 --- a/webots_ros2_driver/CHANGELOG.rst +++ b/webots_ros2_driver/CHANGELOG.rst @@ -5,6 +5,7 @@ Changelog for package webots_ros2_driver 2023.0.4 (2023-XX-XX) ------------------ * Added support for painted point clouds +* Added parameters to rename Camera and RangeFinder topics. 2023.0.3 (2023-04-12) ------------------ diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp index 9f7e7ace1..e1ed0376d 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp @@ -55,6 +55,9 @@ namespace webots_ros2_driver { WbDeviceTag mCamera; + std::string mCameraInfoSuffix; + std::string mImageSuffix; + rclcpp::Publisher::SharedPtr mImagePublisher; sensor_msgs::msg::Image mImageMessage; diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp index 3f2821436..949bb3170 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp @@ -36,6 +36,10 @@ namespace webots_ros2_driver { WbDeviceTag mRangeFinder; + std::string mCameraInfoSuffix; + std::string mImageSuffix; + std::string mPointCloudSuffix; + rclcpp::Publisher::SharedPtr mImagePublisher; sensor_msgs::msg::Image mImageMessage; rclcpp::Publisher::SharedPtr mCameraInfoPublisher; diff --git a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp index 9b131996e..3c28d1355 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp @@ -23,10 +23,14 @@ namespace webots_ros2_driver { mRecognitionIsEnabled = false; mCamera = wb_robot_get_device(parameters["name"].c_str()); + mCameraInfoSuffix = parameters.count("cameraInfoSuffix") ? parameters["cameraInfoSuffix"] : "/camera_info"; + mImageSuffix = parameters.count("imageSuffix") ? parameters["imageSuffix"] : ""; + assert(mCamera != 0); // Image publisher - mImagePublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); + mImagePublisher = + mNode->create_publisher(mTopicName + mImageSuffix, rclcpp::SensorDataQoS().reliable()); mImageMessage.header.frame_id = mFrameName; mImageMessage.height = wb_camera_get_height(mCamera); mImageMessage.width = wb_camera_get_width(mCamera); @@ -37,7 +41,7 @@ namespace webots_ros2_driver { // CameraInfo publisher mCameraInfoPublisher = - mNode->create_publisher(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable()); + mNode->create_publisher(mTopicName + mCameraInfoSuffix, rclcpp::SensorDataQoS().reliable()); mCameraInfoMessage.header.stamp = mNode->get_clock()->now(); mCameraInfoMessage.header.frame_id = mFrameName; mCameraInfoMessage.height = wb_camera_get_height(mCamera); diff --git a/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp b/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp index ca2d564b8..18f4b3d82 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp @@ -25,13 +25,18 @@ namespace webots_ros2_driver { mIsEnabled = false; mRangeFinder = wb_robot_get_device(parameters["name"].c_str()); + mCameraInfoSuffix = parameters.count("cameraInfoSuffix") ? parameters["cameraInfoSuffix"] : "/camera_info"; + mImageSuffix = parameters.count("imageSuffix") ? parameters["imageSuffix"] : ""; + mPointCloudSuffix = parameters.count("pointCloudSuffix") ? parameters["pointCloudSuffix"] : "/point_cloud"; + assert(mRangeFinder != 0); const int width = wb_range_finder_get_width(mRangeFinder); const int height = wb_range_finder_get_height(mRangeFinder); // Image publisher - mImagePublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); + mImagePublisher = + mNode->create_publisher(mTopicName + mImageSuffix, rclcpp::SensorDataQoS().reliable()); mImageMessage.header.frame_id = mFrameName; mImageMessage.height = height; mImageMessage.width = width; @@ -42,7 +47,7 @@ namespace webots_ros2_driver { // CameraInfo publisher mCameraInfoPublisher = - mNode->create_publisher(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable()); + mNode->create_publisher(mTopicName + mCameraInfoSuffix, rclcpp::SensorDataQoS().reliable()); mCameraInfoMessage.header.stamp = mNode->get_clock()->now(); mCameraInfoMessage.header.frame_id = mFrameName; mCameraInfoMessage.height = height; @@ -57,8 +62,8 @@ namespace webots_ros2_driver { 1.0, 0.0}; // Point cloud publisher - mPointCloudPublisher = - mNode->create_publisher(mTopicName + "/point_cloud", rclcpp::SensorDataQoS().reliable()); + mPointCloudPublisher = mNode->create_publisher(mTopicName + mPointCloudSuffix, + rclcpp::SensorDataQoS().reliable()); mPointCloudMessage.header.frame_id = mFrameName; mPointCloudMessage.fields.resize(3); mPointCloudMessage.fields[0].name = "x"; diff --git a/webots_ros2_tiago/CHANGELOG.rst b/webots_ros2_tiago/CHANGELOG.rst index 750553ec1..f72314762 100644 --- a/webots_ros2_tiago/CHANGELOG.rst +++ b/webots_ros2_tiago/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_tiago ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.0.4 (2023-XX-XX) +------------------ +* Added new world, resources and launch file to start the TIAGo with real robot configuration. + 2023.0.3 (2023-04-12) ------------------ * Fixed the calibration of the TIAGo. diff --git a/webots_ros2_tiago/launch/robot_bringup_launch.py b/webots_ros2_tiago/launch/robot_bringup_launch.py new file mode 100644 index 000000000..16303a607 --- /dev/null +++ b/webots_ros2_tiago/launch/robot_bringup_launch.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python + +# Copyright 1996-2023 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch Webots and the controller.""" + +import os +import pathlib +import launch +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch.substitutions.path_join_substitution import PathJoinSubstitution +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription +from webots_ros2_driver.webots_launcher import WebotsLauncher +from webots_ros2_driver.utils import controller_url_prefix + + +def launch_spawners(event, nodes): + # Start ros2_control spawners once the controller_manager is ready + if 'Successful \'activate\' of hardware' in event.text.decode().strip(): + return nodes + return + + +def get_ros2_nodes(*args): + package_dir = get_package_share_directory('webots_ros2_tiago') + use_rviz = LaunchConfiguration('rviz', default=False) + robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'tiago_bringup_webots.urdf')).read_text() + ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control_bringup.yml') + use_sim_time = LaunchConfiguration('use_sim_time', default=True) + + tiago_driver = Node( + package='webots_ros2_driver', + executable='driver', + output='screen', + additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'Tiago'}, + parameters=[ + {'robot_description': robot_description, + 'use_sim_time': use_sim_time}, + ros2_control_params + ] + ) + + tiago_bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('tiago_bringup'), 'launch', 'tiago_bringup.launch.py')), + ) + + spawners = [] + spawners.append(tiago_bringup_launch) + spawners_handler = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessIO( + target_action=tiago_driver, + on_stderr=lambda event: launch_spawners(event, spawners) + ) + ) + + # RViz + rviz_config = os.path.join(get_package_share_directory('webots_ros2_tiago'), 'resource', 'default_bringup.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) + ) + + return [ + tiago_driver, + spawners_handler, + rviz, + ] + + +def generate_launch_description(): + package_dir = get_package_share_directory('webots_ros2_tiago') + world = LaunchConfiguration('world') + mode = LaunchConfiguration('mode') + + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]), + mode=mode, + ros2_supervisor=True + ) + + # The following lines are important! + # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). + reset_handler = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=webots._supervisor, + on_exit=get_ros2_nodes, + ) + ) + + return LaunchDescription([ + DeclareLaunchArgument( + 'world', + default_value='default_bringup.wbt', + description='Choose one of the world files from `/webots_ros2_tiago/world` directory' + ), + DeclareLaunchArgument( + 'mode', + default_value='realtime', + description='Webots startup mode' + ), + webots, + webots._supervisor, + + # This action will kill all nodes once the Webots simulation has exited + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=webots, + on_exit=[ + launch.actions.UnregisterEventHandler( + event_handler=reset_handler.event_handler + ), + launch.actions.EmitEvent(event=launch.events.Shutdown()) + ], + ) + ), + + # Add the reset event handler + reset_handler + ] + get_ros2_nodes()) diff --git a/webots_ros2_tiago/launch/robot_launch.py b/webots_ros2_tiago/launch/robot_launch.py index 21bb8a379..8b51a51e1 100644 --- a/webots_ros2_tiago/launch/robot_launch.py +++ b/webots_ros2_tiago/launch/robot_launch.py @@ -31,8 +31,14 @@ from webots_ros2_driver.utils import controller_url_prefix +def launch_spawners(event, nodes): + # Start ros2_control spawners once the controller_manager is ready + if 'Successful \'activate\' of hardware' in event.text.decode().strip(): + return nodes + return + + def get_ros2_nodes(*args): - optional_nodes = [] package_dir = get_package_share_directory('webots_ros2_tiago') use_rviz = LaunchConfiguration('rviz', default=False) use_nav = LaunchConfiguration('nav', default=False) @@ -47,6 +53,25 @@ def get_ros2_nodes(*args): cartographer_config_basename = 'cartographer.lua' use_sim_time = LaunchConfiguration('use_sim_time', default=True) + mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel')] + if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ['humble', 'rolling']: + mappings.append(('/diffdrive_controller/odom', '/odom')) + + tiago_driver = Node( + package='webots_ros2_driver', + executable='driver', + output='screen', + additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'Tiago_Lite'}, + parameters=[ + {'robot_description': robot_description, + 'use_sim_time': use_sim_time, + 'set_robot_state_publisher': True}, + ros2_control_params + ], + remappings=mappings + ) + + # ROS2_control controller_manager_timeout = ['--controller-manager-timeout', '500'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' @@ -68,24 +93,17 @@ def get_ros2_nodes(*args): arguments=['joint_state_broadcaster'] + controller_manager_timeout, ) - mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel')] - if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ['humble', 'rolling']: - mappings.append(('/diffdrive_controller/odom', '/odom')) - - tiago_driver = Node( - package='webots_ros2_driver', - executable='driver', - output='screen', - additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'Tiago_Lite'}, - parameters=[ - {'robot_description': robot_description, - 'use_sim_time': use_sim_time, - 'set_robot_state_publisher': True}, - ros2_control_params - ], - remappings=mappings + spawners = [] + spawners.append(diffdrive_controller_spawner) + spawners.append(joint_state_broadcaster_spawner) + spawners_handler = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessIO( + target_action=tiago_driver, + on_stderr=lambda event: launch_spawners(event, spawners) + ) ) + # State publishers robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -102,6 +120,7 @@ def get_ros2_nodes(*args): arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], ) + # RViz rviz_config = os.path.join(get_package_share_directory('webots_ros2_tiago'), 'resource', 'default.rviz') rviz = Node( package='rviz2', @@ -113,6 +132,7 @@ def get_ros2_nodes(*args): ) # Navigation + optional_nodes = [] if 'nav2_bringup' in get_packages_with_prefixes(): optional_nodes.append(IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -170,8 +190,7 @@ def get_ros2_nodes(*args): ) return [ - joint_state_broadcaster_spawner, - diffdrive_controller_spawner, + spawners_handler, nav_handler, robot_state_publisher, tiago_driver, diff --git a/webots_ros2_tiago/resource/default_bringup.rviz b/webots_ros2_tiago/resource/default_bringup.rviz new file mode 100644 index 000000000..1eb1e7c06 --- /dev/null +++ b/webots_ros2_tiago/resource/default_bringup.rviz @@ -0,0 +1,168 @@ +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 + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Update Interval: 0 + Value: true + - 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: base_link + 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/webots_ros2_tiago/resource/ros2_control_bringup.yml b/webots_ros2_tiago/resource/ros2_control_bringup.yml new file mode 100644 index 000000000..5199a1954 --- /dev/null +++ b/webots_ros2_tiago/resource/ros2_control_bringup.yml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 50 diff --git a/webots_ros2_tiago/resource/tiago_bringup_webots.urdf b/webots_ros2_tiago/resource/tiago_bringup_webots.urdf new file mode 100644 index 000000000..b130587a1 --- /dev/null +++ b/webots_ros2_tiago/resource/tiago_bringup_webots.urdf @@ -0,0 +1,97 @@ + + + + + + true + 10 + /scan_raw + true + hokuyo + + + + + true + /head_front_camera/rgb + /image_raw + true + head_front_camera_rgb_optical_frame + + + + + true + /head_front_camera/depth_registered + /image_raw + /points + true + head_front_camera_depth_optical_frame + + + + + + + + + webots_ros2_control::Ros2ControlSystem + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/webots_ros2_tiago/setup.py b/webots_ros2_tiago/setup.py index f6e8308c3..355102ca4 100644 --- a/webots_ros2_tiago/setup.py +++ b/webots_ros2_tiago/setup.py @@ -4,19 +4,30 @@ data_files = [] data_files.append( ('share/ament_index/resource_index/packages', ['resource/' + package_name])) -data_files.append(('share/' + package_name + '/launch', ['launch/robot_launch.py'])) +data_files.append(('share/' + package_name + '/launch', [ + 'launch/robot_launch.py', + 'launch/robot_bringup_launch.py' +])) data_files.append(('share/' + package_name + '/resource', [ 'resource/tiago_webots.urdf', + 'resource/tiago_bringup_webots.urdf', 'resource/ros2_control.yml', + 'resource/ros2_control_bringup.yml', 'resource/nav2_params.yaml', 'resource/default.rviz', + 'resource/default_bringup.rviz', 'resource/map.pgm', 'resource/map.yaml', 'resource/cartographer.lua', 'resource/slam_toolbox_params.yaml', ])) data_files.append(('share/' + package_name, ['package.xml'])) -data_files.append(('share/' + package_name + '/worlds', ['worlds/default.wbt', 'worlds/.default.wbproj'])) +data_files.append(('share/' + package_name + '/worlds', [ + 'worlds/default.wbt', + 'worlds/.default.wbproj', + 'worlds/default_bringup.wbt', + 'worlds/.default_bringup.wbproj' +])) setup( name=package_name, diff --git a/webots_ros2_tiago/worlds/.default.wbproj b/webots_ros2_tiago/worlds/.default.wbproj index 7054854cb..046920833 100644 --- a/webots_ros2_tiago/worlds/.default.wbproj +++ b/webots_ros2_tiago/worlds/.default.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021b +Webots Project File version R2023b perspectives: 000000ff00000000fd00000002000000010000011c000002f0fc0200000001fb0000001400540065007800740045006400690074006f00720000000016000002f00000003f00ffffff00000003000004e200000039fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000004e20000006900ffffff000004e20000015700000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff0000000100000002000000000000077e0100000002010000000100 sceneTreePerspectives: 000000ff00000001000000020000007d000000fa0100000002010000000200 @@ -8,5 +8,3 @@ orthographicViewHeight: 1 textFiles: -1 globalOptionalRendering: LidarPointClouds::LidarRaysPaths consoles: Console:All:All -renderingDevicePerspectives: TIAGo Iron:camera;0;1.56;0;0 -renderingDevicePerspectives: TIAGo Iron:display;0;1.56;0;0 diff --git a/webots_ros2_tiago/worlds/.default_bringup.wbproj b/webots_ros2_tiago/worlds/.default_bringup.wbproj new file mode 100644 index 000000000..046920833 --- /dev/null +++ b/webots_ros2_tiago/worlds/.default_bringup.wbproj @@ -0,0 +1,10 @@ +Webots Project File version R2023b +perspectives: 000000ff00000000fd00000002000000010000011c000002f0fc0200000001fb0000001400540065007800740045006400690074006f00720000000016000002f00000003f00ffffff00000003000004e200000039fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000004e20000006900ffffff000004e20000015700000001000000020000000100000008fc00000000 +simulationViewPerspectives: 000000ff0000000100000002000000000000077e0100000002010000000100 +sceneTreePerspectives: 000000ff00000001000000020000007d000000fa0100000002010000000200 +maximizedDockId: -1 +centralWidgetVisible: 1 +orthographicViewHeight: 1 +textFiles: -1 +globalOptionalRendering: LidarPointClouds::LidarRaysPaths +consoles: Console:All:All diff --git a/webots_ros2_tiago/worlds/default_bringup.wbt b/webots_ros2_tiago/worlds/default_bringup.wbt new file mode 100644 index 000000000..a7679f45f --- /dev/null +++ b/webots_ros2_tiago/worlds/default_bringup.wbt @@ -0,0 +1,697 @@ +#VRML_SIM R2023b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/appearances/protos/Parquetry.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/lights/protos/CeilingLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/apartment_structure/protos/Wall.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/appearances/protos/Roughcast.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/apartment_structure/protos/Window.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/appearances/protos/MattePaint.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/apartment_structure/protos/Door.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/living_room_furniture/protos/Sofa.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/plants/protos/PottedTree.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/cabinet/protos/Cabinet.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/cabinet/protos/CabinetHandle.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/appearances/protos/GlossyPaint.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/solids/protos/SolidBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/kitchen/components/protos/Sink.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/tables/protos/Table.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/appearances/protos/VarnishedPine.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/computers/protos/Monitor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/computers/protos/Keyboard.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/chairs/protos/OfficeChair.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/robots/pal_robotics/tiago/protos/Tiago.proto" + +WorldInfo { + info [ + "An office break room, surrounded by office desks." + ] + basicTimeStep 20 +} +Viewpoint { + fieldOfView 0.5 + orientation 0.3179803094444856 0.038784817888461866 -0.9473036792428013 2.9187109824608912 + position 27.15248440605859 6.083643329439582 22.02228189083543 + near 1 + follow "TIAGo Iron" + followType "None" + followSmoothness 0 +} +TexturedBackground { + texture "entrance_hall" + luminosity 0.5 + skybox FALSE +} +Floor { + translation -0.93 0.243 0 + size 7.7 12.86 + appearance Parquetry { + type "light strip" + textureTransform TextureTransform { + rotation 1.57 + scale 0.4 0.4 + } + } +} +CeilingLight { + translation 0.87 4.78 2.5 + rotation 1 0 0 4.692820414042842e-06 + bulbColor 0.913725 0.72549 0.431373 + supportColor 0.533333 0.541176 0.521569 + pointLightIntensity 3 + pointLightCastShadows TRUE +} +CeilingLight { + translation -2.74 4.78 2.5 + rotation 1 0 0 4.692820414042842e-06 + name "ceiling light(5)" + bulbColor 0.913725 0.72549 0.431373 + supportColor 0.533333 0.541176 0.521569 + pointLightIntensity 3 + castShadows FALSE +} +CeilingLight { + translation 0.87 0.31 2.5 + rotation 1 0 0 4.692820414042842e-06 + name "ceiling light(1)" + bulbColor 0.913725 0.72549 0.431373 + supportColor 0.533333 0.541176 0.521569 + pointLightIntensity 3 + castShadows FALSE +} +CeilingLight { + translation -2.74 -3.52 2.5 + rotation 1 0 0 4.692820414042842e-06 + name "ceiling light(3)" + bulbColor 0.913725 0.72549 0.431373 + supportColor 0.533333 0.541176 0.521569 + pointLightIntensity 3 + pointLightCastShadows TRUE + castShadows FALSE +} +Wall { + translation -2.92 -2.65 0 + size 3.5 0.05 0.62 + appearance DEF SMALL_WALLS Roughcast { + colorOverride 0.890196 0.803922 1 + textureTransform TextureTransform { + scale 1 2.4 + } + } +} +Wall { + translation 0.77 2.65 0 + name "wall(1)" + size 3.55 0.05 0.62 + appearance USE SMALL_WALLS +} +Wall { + translation 2.52 0 0 + rotation 7.19232e-09 7.19235e-09 -1 -1.5707953071795862 + name "wall(2)" + size 5.35 0.05 0.62 + appearance USE SMALL_WALLS +} +Wall { + translation -0.979 3.026 0 + rotation 0 0 1 1.5708 + name "wall(7)" + size 0.7 0.05 0.62 + appearance USE SMALL_WALLS +} +Wall { + translation -0.92 -6.17 0 + rotation -0.788631751825513 -5.672108215044546e-09 -0.6148658065079098 -5.307179586466759e-06 + name "wall(A)" + size 7.7 0.06 2.5 + appearance DEF SIDE_WALLS Roughcast { + textureTransform TextureTransform { + scale 3 3 + } + } +} +Wall { + translation 2.9 -5.94 0 + rotation 0 0 1 1.5708 + name "wall(B)1" + size 0.4 0.06 2.5 + appearance DEF INTER_WALLS Roughcast { + textureTransform TextureTransform { + scale 0.3 2.2 + } + } +} +Window { + translation 2.9 -5.19 0 + rotation -1.1201004593060733e-08 -0.005401822215060023 0.9999854100519451 -5.307179586466759e-06 + name "window(B)2" + size 0.06 1.1 2.5 + bottomWallHeight 1 + frameSize 0.02 0.05 0.02 + windowSillSize 0.3 0.05 + frameAppearance DEF WINDOWS_WOOD MattePaint { + baseColor 0.133333 0.0666667 0 + } +} +Window { + translation 2.9 -3.84 0 + rotation 2.9524886867359215e-08 0.014173193695777315 0.9998995552456563 1.01503e-06 + name "window(B)3" + size 0.06 1.6 2.5 + bottomWallHeight 1 + frameSize 0.02 0.05 0.02 + windowSillSize 0.3 0.05 + frameAppearance USE WINDOWS_WOOD +} +Wall { + translation 2.9 -2.84 0 + rotation 0 0 1 1.5708 + name "wall(B)4" + size 0.4 0.06 2.5 + appearance USE INTER_WALLS +} +Window { + translation 2.9 -2.09 0 + rotation 2.9524886867359215e-08 0.014173193695777315 0.9998995552456563 1.01503e-06 + name "window(B)5" + size 0.06 1.1 2.5 + bottomWallHeight 1 + frameSize 0.02 0.05 0.02 + windowSillSize 0.3 0.05 + frameAppearance USE WINDOWS_WOOD +} +Window { + translation 2.9 -0.74 0 + rotation 2.9524886867359215e-08 0.014173193695777315 0.9998995552456563 1.01503e-06 + name "window(B)6" + size 0.06 1.6 2.5 + bottomWallHeight 1 + frameSize 0.02 0.05 0.02 + windowSillSize 0.3 0.05 + frameAppearance USE WINDOWS_WOOD +} +Wall { + translation 2.9 0.26 0 + rotation 0 0 1 1.5708 + name "wall(B)7" + size 0.4 0.06 2.5 + appearance USE INTER_WALLS +} +Window { + translation 2.9 1 0 + rotation 0.12721696286162107 0.4975208547590069 0.8580727495032879 1.01503e-06 + name "window(B)8" + size 0.06 1.1 2.5 + bottomWallHeight 1 + frameSize 0.02 0.05 0.02 + windowSillSize 0.3 0.05 + frameAppearance USE WINDOWS_WOOD +} +Window { + translation 2.9 2.35 0 + rotation 2.9524886867359215e-08 0.014173193695777315 0.9998995552456563 1.01503e-06 + name "window(B)9" + size 0.06 1.6 2.5 + bottomWallHeight 1 + frameSize 0.02 0.05 0.02 + windowSillSize 0.3 0.05 + frameAppearance USE WINDOWS_WOOD +} +Wall { + translation 2.9 3.35 0 + rotation 0 0 1 1.5708 + name "wall(B)10" + size 0.4 0.06 2.5 + appearance USE INTER_WALLS +} +Window { + translation 2.9 4.1 0 + rotation 2.9524886867359215e-08 0.014173193695777315 0.9998995552456563 1.01503e-06 + name "window(B)11" + size 0.06 1.1 2.5 + bottomWallHeight 1 + frameSize 0.02 0.05 0.02 + windowSillSize 0.3 0.05 + frameAppearance USE WINDOWS_WOOD +} +Window { + translation 2.9 5.45 0 + rotation 2.9524886867359215e-08 0.014173193695777315 0.9998995552456563 1.01503e-06 + name "window(B)12" + size 0.06 1.6 2.5 + bottomWallHeight 1 + frameSize 0.02 0.05 0.02 + windowSillSize 0.3 0.05 + frameAppearance USE WINDOWS_WOOD +} +Wall { + translation 2.9 6.45 0 + rotation 0 0 1 1.5708 + name "wall(B)13" + size 0.4 0.06 2.5 + appearance USE INTER_WALLS +} +Wall { + translation -0.92 6.68 0 + rotation 0 0 1 3.14159 + name "wall(C)" + size 7.7 0.06 2.5 + appearance USE SIDE_WALLS +} +Wall { + translation -4.8 5.71 0 + rotation 0 0 1 -1.5708 + name "wall(D)1" + size 2 0.06 2.5 +} +Door { + translation -4.8 4.21 0 + rotation -2.827660768165793e-11 0.003931491068033687 -0.9999922716591273 -5.307179586466759e-06 + size 0.06 1 2.5 + frameSize 0.05 0.05 0.01 + frameAppearance MattePaint { + baseColor 0.133333 0.0666667 0 + } +} +Wall { + translation -4.8 -1.24 0 + rotation 2.9524886867359215e-08 0.014173193695777315 0.9998995552456563 1.01503e-06 + name "wall(D)2" + size 0.06 9.9 2.5 +} +Sofa { + translation 2 1.22999 0 + rotation 9.22314e-15 7.19235e-09 -1 -3.1415853071795863 +} +PottedTree { + translation 2.15 2.38 0 + rotation 2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 1.5708 +} +PottedTree { + translation -1.42 6.23 0 + rotation 2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 1.5708 + name "potted tree(1)" +} +PottedTree { + translation 2.17 -5.78 0 + rotation -2.3464199999870813e-06 2.3464199999870813e-06 -0.9999999999944944 -1.5707953071795862 + name "potted tree(2)" +} +Cabinet { + translation 2.49 -0.01 0 + rotation 7.19233e-09 -1.49483e-14 1 3.14159 + outerThickness 0.02 + rowsHeights [ + 0.3, 0.4, 0.3, 0.3 + ] + columnsWidths [ + 0.6 + ] + layout [ + "Drawer (1, 1, 1, 1, 3.5)" + "Shelf (1, 4, 1, 0)" + "Shelf (1, 3, 1, 0)" + "Shelf (1, 2, 1, 0)" + "Shelf (1, 1, 1, 1)" + ] + handle CabinetHandle { + handleColor 0.533333 0.541176 0.521569 + } + primaryAppearance MattePaint { + baseColor 0.729412 0.741176 0.713725 + } + secondaryAppearance GlossyPaint { + baseColor 0.643137 0 0 + } +} +Cabinet { + translation -0.22 2.62 0 + rotation -2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 -1.5707953071795862 + name "cabinet(1)" + outerThickness 0.02 + rowsHeights [ + 0.1, 0.25 + ] + columnsWidths [ + 0.5, 0.5 + ] + layout [ + "Drawer (2, 2, 1, 1, 1.5)" + "Drawer (1, 2, 1, 1, 1.5)" + "Shelf (1, 1, 0, 2)" + "Shelf (2, 2, 1, 0)" + "Shelf (1, 2, 1, 0)" + "Shelf (1, 1, 0, 1)" + ] + handle CabinetHandle { + handleColor 0.533333 0.541176 0.521569 + } + primaryAppearance MattePaint { + baseColor 0.729412 0.741176 0.713725 + } + secondaryAppearance GlossyPaint { + baseColor 0.643137 0 0 + } +} +Cabinet { + translation 0.7 6.65 1.6 + rotation 2.7023499999809715e-06 2.6039199999816646e-06 0.9999999999929585 -1.5707953071795862 + name "cabinet(2)" + outerThickness 0.02 + rowsHeights [ + 0.8 + ] + columnsWidths [ + 0.8, 0.8, 0.8, 0.8 + ] + layout [ + "Drawer (1, 1, 1, 1, 1.5)" + "Drawer (2, 1, 1, 1, 1.5)" + "Drawer (3, 1, 1, 1, 1.5)" + "Drawer (4, 1, 1, 1, 1.5)" + "Shelf (1, 1, 0, 1)" + "Shelf (2, 1, 0, 1)" + "Shelf (3, 1, 0, 1)" + ] + handle CabinetHandle { + handleColor 0.533333 0.541176 0.521569 + } + primaryAppearance MattePaint { + baseColor 0.729412 0.741176 0.713725 + } + secondaryAppearance MattePaint { + baseColor 0.666667 0.333333 0 + } +} +Cabinet { + translation 0.7 -6.14 1.6 + rotation -1.67821e-08 1.67821e-08 -1 -1.5707953071795862 + name "cabinet(3)" + outerThickness 0.02 + rowsHeights [ + 0.8 + ] + columnsWidths [ + 0.8, 0.8, 0.8, 0.8 + ] + layout [ + "Drawer (1, 1, 1, 1, 1.5)" + "Drawer (2, 1, 1, 1, 1.5)" + "Drawer (3, 1, 1, 1, 1.5)" + "Drawer (4, 1, 1, 1, 1.5)" + "Shelf (1, 1, 0, 1)" + "Shelf (2, 1, 0, 1)" + "Shelf (3, 1, 0, 1)" + ] + handle CabinetHandle { + handleColor 0.533333 0.541176 0.521569 + } + primaryAppearance MattePaint { + baseColor 0.729412 0.741176 0.713725 + } + secondaryAppearance MattePaint { + baseColor 0.666667 0.333333 0 + } +} +Cabinet { + translation 2.49 -0.66 0 + rotation 7.19233e-09 -1.49483e-14 1 3.14159 + name "cabinet(5)" + outerThickness 0.02 + rowsHeights [ + 0.7, 0.3, 0.3 + ] + columnsWidths [ + 0.6 + ] + layout [ + "LeftSidedDoor (1, 1, 1, 1, 1.5)" + "Shelf (1, 3, 1, 0)" + "Shelf (1, 2, 1, 0)" + "Shelf (1, 1, 1, 1)" + ] + handle CabinetHandle { + handleColor 0.533333 0.541176 0.521569 + } + primaryAppearance MattePaint { + baseColor 0.729412 0.741176 0.713725 + } + secondaryAppearance GlossyPaint { + baseColor 0.643137 0 0 + } +} +Cabinet { + translation 2.5 -1.52 0.1 + rotation 7.19233e-09 -1.49483e-14 1 3.14159 + name "cabinet(6)" + outerThickness 0.02 + rowsHeights [ + 0.22, 0.22, 0.22 + ] + columnsWidths [ + 0.5, 0.5 + ] + layout [ + "LeftSidedDoor (2, 1, 1, 3, 1.5)" + "LeftSidedDoor (1, 1, 1, 3, 1.5)" + "Shelf (1, 1, 0, 3)" + "Shelf (2, 1, 0, 3)" + ] + handle CabinetHandle { + translation 0 0.26 0.02 + handleLength 0.1 + handleRadius 0.008 + handleColor 0.427451 0.513725 0.533333 + } + primaryAppearance MattePaint { + baseColor 0.94667 0.925551 0.852003 + } + secondaryAppearance MattePaint { + baseColor 0.94667 0.925551 0.852003 + } +} +Cabinet { + translation 2.5 -2.31 0.1 + rotation 7.19233e-09 -1.49483e-14 1 3.14159 + name "cabinet(7)" + outerThickness 0.02 + rowsHeights [ + 0.22, 0.22, 0.22 + ] + columnsWidths [ + 0.5 + ] + layout [ + "Drawer (1, 3, 1, 1, 1)" + "Drawer (1, 2, 1, 1, 1)" + "Drawer (1, 1, 1, 1, 1)" + "Shelf (1, 1, 0, 3)" + ] + handle CabinetHandle { + translation 0.02 0.01 0 + handleLength 0.1 + handleRadius 0.008 + handleColor 0.427451 0.513725 0.533333 + } + primaryAppearance MattePaint { + baseColor 0.94667 0.925551 0.852003 + } + secondaryAppearance MattePaint { + baseColor 0.94667 0.925551 0.852003 + } +} +SolidBox { + translation 2.25 -1.79 0.05 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + size 1.58 0.1 0.5 + appearance PBRAppearance { + baseColor 0.8 0.8 0.8 + roughness 0.5 + metalness 0 + textureTransform TextureTransform { + scale 4 4 + } + } +} +Sink { + translation 2.32 -2.27 0.835 + rotation -7.19233e-09 -1.50172e-14 1 3.14159 +} +Table { + translation 1.66 3.18 0 + rotation 2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 1.5708 + size 1 1.6 0.74 + feetSize 0.05 0.05 + trayAppearance DEF TABLE_WOOD VarnishedPine { + textureTransform TextureTransform { + scale 10 10 + } + } + legAppearance MattePaint { + baseColor 0.2 0.2 0.2 + } +} +Table { + translation 1.66 6.14 0 + rotation -2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 -1.5707953071795862 + name "table(1)" + size 1 1.6 0.74 + feetSize 0.05 0.05 + legAppearance MattePaint { + baseColor 0.2 0.2 0.2 + } +} +Table { + translation -0.05 3.18 0 + rotation 2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 1.5708 + name "table(3)" + feetSize 0.05 0.05 + legAppearance MattePaint { + baseColor 0.2 0.2 0.2 + } +} +Table { + translation -0.05 6.14 0 + rotation -2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 -1.5707953071795862 + name "table(4)" + feetSize 0.05 0.05 + legAppearance MattePaint { + baseColor 0.2 0.2 0.2 + } +} +Table { + translation -2.03 -3.18 0 + rotation -2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 -1.5707953071795862 + name "table(5)" + feetSize 0.05 0.05 + legAppearance MattePaint { + baseColor 0.2 0.2 0.2 + } +} +Table { + translation -3.83 -3.18 0 + rotation -2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 -1.5707953071795862 + name "table(6)" + feetSize 0.05 0.05 + legAppearance MattePaint { + baseColor 0.2 0.2 0.2 + } +} +Monitor { + translation -3.82817 -3.02267 0.76 + rotation 1.50576e-07 -1.47902e-07 1 -1.8325853071795866 +} +Monitor { + translation 1.79048 6.47928 0.76 + rotation -7.19236e-09 -6.30754e-09 1 -1.701685307179586 + name "monitor(1)" +} +Monitor { + translation -4.33723 -3.01523 0.76 + rotation 7.19236e-09 9.3732e-09 -1 1.309 + name "monitor(2)" +} +Monitor { + translation 1.29031 6.47723 0.76 + rotation 7.19237e-09 8.20135e-09 -1 1.43989 + name "monitor(3)" +} +Monitor { + translation -1.65969 -2.84277 0.76 + rotation -7.19233e-09 -6.30755e-09 1 -1.7016953071795866 + name "monitor(6)" +} +Monitor { + translation -2.14969 -2.84277 0.76 + rotation 7.19237e-09 8.20135e-09 -1 1.43989 + name "monitor(7)" +} +Monitor { + translation 1.79048 2.87928 0.76 + rotation -7.19212e-09 6.30732e-09 1 1.70169 + name "monitor(8)" +} +Monitor { + translation 1.29031 2.87723 0.76 + rotation -7.19209e-09 8.201e-09 1 1.4399 + name "monitor(9)" +} +Monitor { + translation 0.18031 2.87723 0.76 + rotation -7.19215e-09 6.30735e-09 1 1.70169 + name "monitor(10)" +} +Monitor { + translation -0.35969 2.87723 0.76 + rotation -7.19211e-09 8.20104e-09 1 1.4399 + name "monitor(11)" +} +Keyboard { + translation -4.02533 -3.20123 0.74 + rotation -7.19237e-09 8.20127e-09 -1 -1.4398953071795866 +} +Keyboard { + translation 1.57 6.05 0.74 + rotation 2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 1.5708 + name "keyboard(1)" +} +Keyboard { + translation -1.91 -3.26 0.74 + rotation -7.19233e-09 -7.19233e-09 1 -1.5707953071795862 + name "keyboard(3)" +} +Keyboard { + translation 1.5267 2.98306 0.74 + rotation 7.19243e-09 5.51899e-09 1 -1.8325953071795862 + name "keyboard(4)" +} +Keyboard { + translation -0.1 3.07 0.74 + rotation -2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 -1.5707953071795862 + name "keyboard(5)" +} +OfficeChair { + translation 0.0449998 4.01794 0 + rotation 7.19235e-09 4.15252e-09 1 -2.094395307179586 +} +OfficeChair { + translation 1.53 3.94 0 + rotation -2.3464099999870814e-06 -2.3464099999870814e-06 0.9999999999944944 -1.5707953071795862 + name "office chair(1)" +} +OfficeChair { + translation 1.48338 5.36388 0 + rotation 7.19235e-09 -5.5189e-09 1 1.83259 + name "office chair(2)" +} +OfficeChair { + translation -3.88082 -4.17593 0 + rotation -7.19235e-09 7.19237e-09 -1 -1.5707953071795862 + name "office chair(3)" +} +OfficeChair { + translation -1.85 -4.07 0 + rotation 7.19235e-09 -2.97918e-09 1 2.35619 + name "office chair(5)" +} +Tiago { + translation 0.962153 -4.80117 0.235361 + rotation -0.006363227570975 -0.002646197037686 -0.999976253206104 0.78447116344273 + name "Tiago" + controller "" + supervisor TRUE + lidarSlot [ + Lidar { + name "hokuyo" + horizontalResolution 660 + fieldOfView 3.14 + numberOfLayers 1 + maxRange 6 + } + Display { + width 800 + height 600 + } + ] +}