From 61db58ed246bcad865ae3a312b470f48c053584c Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Tue, 2 Apr 2019 19:03:05 +0200 Subject: [PATCH 01/25] first step to gazebo support rename links and joints to have semantic names add inertia to all links add laser and diffdrive plugins Signed-off-by: Mikael Arguedas --- confbot_description/urdf/confbot.urdf | 195 ++++++++++++++++++++++++-- 1 file changed, 186 insertions(+), 9 deletions(-) diff --git a/confbot_description/urdf/confbot.urdf b/confbot_description/urdf/confbot.urdf index 6382c82..9877ec3 100644 --- a/confbot_description/urdf/confbot.urdf +++ b/confbot_description/urdf/confbot.urdf @@ -11,19 +11,45 @@ + + + + + + + + + + + + + - + + - + - + + + + + + + + + + + + + @@ -32,33 +58,184 @@ - - + + + + + + + + + + + + + + + + - + - - + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + / + cmd_vel:=cmd_vel + odom:=odom + + + + wheel_l_joint + wheel_r_joint + + + 1.2 + 0.6 + + + 20 + 1.0 + + + true + true + true + + odom + base_link + + + + + + + + false + 3 + + + + 1875 + 1 + -2.0943951023931953 + 2.0943951023931953 + + + + 0.1 + 30.0 + 0.001 + + + gaussian + 0.0 + 0.008 + + + + + sensor_msgs/LaserScan + + + / + ~/out:=scan + + + + From 3cdf7a163648ef04ab9e47c4c4665d728186cd3f Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Tue, 2 Apr 2019 19:04:38 +0200 Subject: [PATCH 02/25] add scripts taken and modifies from turtlebot3_simulation Signed-off-by: Mikael Arguedas --- .../launch/confbot_state_publisher.launch.py | 46 +++++++++++++++++++ confbot_tools/confbot_tools/urdf_spawner.py | 32 +++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 confbot_bringup/launch/confbot_state_publisher.launch.py create mode 100644 confbot_tools/confbot_tools/urdf_spawner.py diff --git a/confbot_bringup/launch/confbot_state_publisher.launch.py b/confbot_bringup/launch/confbot_state_publisher.launch.py new file mode 100644 index 0000000..0493641 --- /dev/null +++ b/confbot_bringup/launch/confbot_state_publisher.launch.py @@ -0,0 +1,46 @@ +# /******************************************************************************* +# * Copyright 2019 ROBOTIS CO., 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. +# *******************************************************************************/ + +# /* Author: Darby Lim */ + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration, EnvironmentVariable +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + urdf_file_name = 'confbot.urdf' + + urdf = os.path.join(get_package_share_directory('confbot_description'), 'urdf', urdf_file_name) + + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true'), + + Node( + package='robot_state_publisher', + node_executable='robot_state_publisher', + node_name='robot_state_publisher', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=[urdf]), + ]) diff --git a/confbot_tools/confbot_tools/urdf_spawner.py b/confbot_tools/confbot_tools/urdf_spawner.py new file mode 100644 index 0000000..f8ad1bd --- /dev/null +++ b/confbot_tools/confbot_tools/urdf_spawner.py @@ -0,0 +1,32 @@ +import sys + +from gazebo_msgs.srv import SpawnEntity +import rclpy + + +def request_spawn(xml: str): + rclpy.init() + node = rclpy.create_node('spawn_entity') + client = node.create_client(SpawnEntity, 'spawn_entity') + if not client.service_is_ready(): + print('waiting for service') + client.wait_for_service() + print('service ready, making request') + request = SpawnEntity.Request() + request.xml = xml + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future) + if future.result() is not None: + print('response: %r' % future.result()) + else: + raise RuntimeError('exception while calling service: %r' % future.exception()) + node.destroy_node() + rclpy.shutdown() + + +if len(sys.argv) < 2: + print('usage: ros2 run my_package my_node.py -- example.urdf') + sys.exit(1) + +f = open(sys.argv[1], 'r') +request_spawn(f.read()) From 806188c2ddb9f77bd295dee1c735a11e8bc9e26b Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Tue, 2 Apr 2019 19:06:44 +0200 Subject: [PATCH 03/25] add simulation launchfile Signed-off-by: Mikael Arguedas --- confbot_simulation/CMakeLists.txt | 15 ++++++ .../launch/empty_world.launch.py | 46 +++++++++++++++++++ confbot_simulation/package.xml | 21 +++++++++ 3 files changed, 82 insertions(+) create mode 100644 confbot_simulation/CMakeLists.txt create mode 100644 confbot_simulation/launch/empty_world.launch.py create mode 100644 confbot_simulation/package.xml diff --git a/confbot_simulation/CMakeLists.txt b/confbot_simulation/CMakeLists.txt new file mode 100644 index 0000000..bd33016 --- /dev/null +++ b/confbot_simulation/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.5) +project(confbot_simulation) + +# find dependencies +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py new file mode 100644 index 0000000..9ef0350 --- /dev/null +++ b/confbot_simulation/launch/empty_world.launch.py @@ -0,0 +1,46 @@ +# /******************************************************************************* +# * Copyright 2019 ROBOTIS CO., 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. +# *******************************************************************************/ + +# /* Author: Darby Lim */ + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time', default='True') + launch_file_dir = os.path.join(get_package_share_directory('confbot_bringup'), 'launch') + + return LaunchDescription([ + ExecuteProcess( + cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'], + output='screen'), + + ExecuteProcess( + cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], + output='screen'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([launch_file_dir, '/confbot_state_publisher.launch.py']), + launch_arguments={'use_sim_time': use_sim_time}.items(), + ), + ]) diff --git a/confbot_simulation/package.xml b/confbot_simulation/package.xml new file mode 100644 index 0000000..769cc3f --- /dev/null +++ b/confbot_simulation/package.xml @@ -0,0 +1,21 @@ + + + + confbot_simulation + 0.0.0 + resources for simulating confbot + Mikael Arguedas + Apache 2.0 + + ament_cmake + + gazebo_plugins + gazebo_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 09cba9af614a932ed8ccf2fb84f0425bd4de63c5 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Tue, 2 Apr 2019 19:16:27 +0200 Subject: [PATCH 04/25] move urdf_spawner back in confbot_simulation Signed-off-by: Mikael Arguedas --- confbot_simulation/CMakeLists.txt | 2 ++ .../confbot_simulation}/urdf_spawner.py | 2 ++ 2 files changed, 4 insertions(+) rename {confbot_tools/confbot_tools => confbot_simulation/confbot_simulation}/urdf_spawner.py (97%) diff --git a/confbot_simulation/CMakeLists.txt b/confbot_simulation/CMakeLists.txt index bd33016..ae5dcd1 100644 --- a/confbot_simulation/CMakeLists.txt +++ b/confbot_simulation/CMakeLists.txt @@ -6,6 +6,8 @@ find_package(ament_cmake REQUIRED) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(PROGRAMS ${PROJECT_NAME}/urdf_spawner.py + DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/confbot_tools/confbot_tools/urdf_spawner.py b/confbot_simulation/confbot_simulation/urdf_spawner.py similarity index 97% rename from confbot_tools/confbot_tools/urdf_spawner.py rename to confbot_simulation/confbot_simulation/urdf_spawner.py index f8ad1bd..05c01b5 100644 --- a/confbot_tools/confbot_tools/urdf_spawner.py +++ b/confbot_simulation/confbot_simulation/urdf_spawner.py @@ -1,3 +1,5 @@ +#! /usr/bin/env python3 + import sys from gazebo_msgs.srv import SpawnEntity From d164f5a7d8d363ae099a911846fe665ed061377e Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Tue, 2 Apr 2019 20:37:50 +0200 Subject: [PATCH 05/25] restore package:// path Signed-off-by: Mikael Arguedas --- confbot_description/urdf/confbot.urdf | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/confbot_description/urdf/confbot.urdf b/confbot_description/urdf/confbot.urdf index 9877ec3..7e14170 100644 --- a/confbot_description/urdf/confbot.urdf +++ b/confbot_description/urdf/confbot.urdf @@ -27,8 +27,7 @@ - - + From 7bb59d90dd8e9855e35265d901868c8c96cf0e8a Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Tue, 2 Apr 2019 20:44:10 +0200 Subject: [PATCH 06/25] add dependencies not part of the nightly image Signed-off-by: Mikael Arguedas --- additional_repos.repos | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/additional_repos.repos b/additional_repos.repos index 56f46b6..2657f93 100644 --- a/additional_repos.repos +++ b/additional_repos.repos @@ -1 +1,13 @@ repositories: + gazebo_ros_pkgs: + type: git + url: https://github.com/ros-simulation/gazebo_ros_pkgs.git + version: ros2 + image_common: + type: git + url: https://github.com/ros-perception/image_common.git + version: ros2 + vision_opencv: + type: git + url: https://github.com/ros-perception/vision_opencv.git + version: ros2 From 9b37f60de40d3190bf99350044ea52d998fc4d4a Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Tue, 2 Apr 2019 20:59:36 +0200 Subject: [PATCH 07/25] blacklist dependencies built from source from the test invocation Signed-off-by: Mikael Arguedas --- .travis/ci_script.bash | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100755 .travis/ci_script.bash diff --git a/.travis/ci_script.bash b/.travis/ci_script.bash new file mode 100755 index 0000000..2a7fb48 --- /dev/null +++ b/.travis/ci_script.bash @@ -0,0 +1,25 @@ +#!/bin/bash +set -e + +cd $ROS2_WS + +# install dependencies +rosdep update && apt-get -qq update && rosdep install -y \ + --from-paths src \ + --ignore-src + +# setup ros2 environment +source "/opt/ros/$ROS_DISTRO/setup.bash" + +# build +colcon build \ + --symlink-install \ + --cmake-args -DSECURITY=ON --no-warn-unused-cli +# test +colcon test \ + --executor sequential \ + --packages-skip gazebo_dev gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_pkgs \ + cv_bridge image_geometry opencv_tests vision_opencv \ + camera_calibration_parsers camera_info_manager image_common image_transport polled_camera \ + --event-handlers console_direct+ +colcon test-result From 84cc7d9b8ba089725bd5240b2c7771543dd582b7 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Tue, 2 Apr 2019 21:10:28 +0200 Subject: [PATCH 08/25] fix test failures Signed-off-by: Mikael Arguedas --- .../launch/confbot_state_publisher.launch.py | 33 +++++++++---------- .../confbot_simulation/urdf_spawner.py | 14 ++++++++ .../launch/empty_world.launch.py | 28 ++++++++-------- 3 files changed, 43 insertions(+), 32 deletions(-) diff --git a/confbot_bringup/launch/confbot_state_publisher.launch.py b/confbot_bringup/launch/confbot_state_publisher.launch.py index 0493641..ecf0419 100644 --- a/confbot_bringup/launch/confbot_state_publisher.launch.py +++ b/confbot_bringup/launch/confbot_state_publisher.launch.py @@ -1,18 +1,16 @@ -# /******************************************************************************* -# * Copyright 2019 ROBOTIS CO., 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. -# *******************************************************************************/ +# Copyright 2019 ROBOTIS CO., 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. # /* Author: Darby Lim */ @@ -20,16 +18,17 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.substitutions import LaunchConfiguration, EnvironmentVariable from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node + def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') urdf_file_name = 'confbot.urdf' urdf = os.path.join(get_package_share_directory('confbot_description'), 'urdf', urdf_file_name) - + return LaunchDescription([ DeclareLaunchArgument( 'use_sim_time', diff --git a/confbot_simulation/confbot_simulation/urdf_spawner.py b/confbot_simulation/confbot_simulation/urdf_spawner.py index 05c01b5..bd7f696 100644 --- a/confbot_simulation/confbot_simulation/urdf_spawner.py +++ b/confbot_simulation/confbot_simulation/urdf_spawner.py @@ -1,5 +1,19 @@ #! /usr/bin/env python3 +# Copyright 2014-2015 Open Source Robotics Foundation, Inc. +# +# 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. + import sys from gazebo_msgs.srv import SpawnEntity diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py index 9ef0350..d59d4b3 100644 --- a/confbot_simulation/launch/empty_world.launch.py +++ b/confbot_simulation/launch/empty_world.launch.py @@ -1,18 +1,16 @@ -# /******************************************************************************* -# * Copyright 2019 ROBOTIS CO., 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. -# *******************************************************************************/ +# Copyright 2019 ROBOTIS CO., 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. # /* Author: Darby Lim */ From 1c73252015f45107e55403c3f24a4fd27d878f50 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Tue, 2 Apr 2019 22:21:18 +0200 Subject: [PATCH 09/25] almost empty world Signed-off-by: Mikael Arguedas --- confbot_simulation/CMakeLists.txt | 2 +- .../launch/empty_world.launch.py | 5 +- confbot_simulation/world/basic_world.world | 302 ++++++++++++++++++ 3 files changed, 306 insertions(+), 3 deletions(-) create mode 100644 confbot_simulation/world/basic_world.world diff --git a/confbot_simulation/CMakeLists.txt b/confbot_simulation/CMakeLists.txt index ae5dcd1..36dd53f 100644 --- a/confbot_simulation/CMakeLists.txt +++ b/confbot_simulation/CMakeLists.txt @@ -4,7 +4,7 @@ project(confbot_simulation) # find dependencies find_package(ament_cmake REQUIRED) -install(DIRECTORY launch +install(DIRECTORY launch world DESTINATION share/${PROJECT_NAME}) install(PROGRAMS ${PROJECT_NAME}/urdf_spawner.py DESTINATION lib/${PROJECT_NAME}) diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py index d59d4b3..f07ddff 100644 --- a/confbot_simulation/launch/empty_world.launch.py +++ b/confbot_simulation/launch/empty_world.launch.py @@ -26,11 +26,12 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='True') + world = os.path.join( + get_package_share_directory('confbot_simulation'), 'world', 'basic_world.world') launch_file_dir = os.path.join(get_package_share_directory('confbot_bringup'), 'launch') - return LaunchDescription([ ExecuteProcess( - cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'], + cmd=['gazebo', '--verbose', world, '-s', 'libgazebo_ros_factory.so'], output='screen'), ExecuteProcess( diff --git a/confbot_simulation/world/basic_world.world b/confbot_simulation/world/basic_world.world new file mode 100644 index 0000000..d1e0b6e --- /dev/null +++ b/confbot_simulation/world/basic_world.world @@ -0,0 +1,302 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + -3.60012 0.192366 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + -0.041537 4.12064 0.5 0 -0 0 + + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.5 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + + + + + + + 0 + 0 + 0 + + + + -0.137958 -2.96108 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 0 + + + + 61 6000000 + 61 188312274 + 1554235878 394463354 + 61006 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -3.60012 0.192366 0.499995 0 1e-05 0 + 1 1 1 + + -3.60012 0.192366 0.499995 0 1e-05 0 + 0 0 0 0 -0 0 + 0.010615 -0.006191 -9.78231 0.012424 0.021225 1.8e-05 + 0.010615 -0.006191 -9.78231 0 -0 0 + + + + -0.137956 -2.96108 0.499997 3e-06 4e-06 -0 + 1 1 1 + + -0.137956 -2.96108 0.499997 3e-06 4e-06 -0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + -0.041537 4.12064 0.5 0 -0 0 + 1 1 1 + + -0.041537 4.12064 0.5 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 5 -5 2 0 0.275643 2.35619 + orbit + perspective + + + + From 3aa543d65c67b1972f46ce573c5465aaebe181d2 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Wed, 3 Apr 2019 21:01:33 +0200 Subject: [PATCH 10/25] rebasing Signed-off-by: Mikael Arguedas --- .travis/ci_script.bash | 25 ------------------- .../confbot_simulation/urdf_spawner.py | 2 +- 2 files changed, 1 insertion(+), 26 deletions(-) delete mode 100755 .travis/ci_script.bash diff --git a/.travis/ci_script.bash b/.travis/ci_script.bash deleted file mode 100755 index 2a7fb48..0000000 --- a/.travis/ci_script.bash +++ /dev/null @@ -1,25 +0,0 @@ -#!/bin/bash -set -e - -cd $ROS2_WS - -# install dependencies -rosdep update && apt-get -qq update && rosdep install -y \ - --from-paths src \ - --ignore-src - -# setup ros2 environment -source "/opt/ros/$ROS_DISTRO/setup.bash" - -# build -colcon build \ - --symlink-install \ - --cmake-args -DSECURITY=ON --no-warn-unused-cli -# test -colcon test \ - --executor sequential \ - --packages-skip gazebo_dev gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_pkgs \ - cv_bridge image_geometry opencv_tests vision_opencv \ - camera_calibration_parsers camera_info_manager image_common image_transport polled_camera \ - --event-handlers console_direct+ -colcon test-result diff --git a/confbot_simulation/confbot_simulation/urdf_spawner.py b/confbot_simulation/confbot_simulation/urdf_spawner.py index bd7f696..16be221 100644 --- a/confbot_simulation/confbot_simulation/urdf_spawner.py +++ b/confbot_simulation/confbot_simulation/urdf_spawner.py @@ -1,6 +1,6 @@ #! /usr/bin/env python3 -# Copyright 2014-2015 Open Source Robotics Foundation, Inc. +# Copyright 2019 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. From 60deaa45d9bae47ad76c6173896a9604f8c69ccc Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Sat, 4 May 2019 12:17:35 +0200 Subject: [PATCH 11/25] dont set use_sim_time as currently doesn't impact time used Signed-off-by: Mikael Arguedas --- confbot_simulation/launch/empty_world.launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py index f07ddff..9695412 100644 --- a/confbot_simulation/launch/empty_world.launch.py +++ b/confbot_simulation/launch/empty_world.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='True') + use_sim_time = LaunchConfiguration('use_sim_time', default='False') world = os.path.join( get_package_share_directory('confbot_simulation'), 'world', 'basic_world.world') launch_file_dir = os.path.join(get_package_share_directory('confbot_bringup'), 'launch') @@ -34,9 +34,9 @@ def generate_launch_description(): cmd=['gazebo', '--verbose', world, '-s', 'libgazebo_ros_factory.so'], output='screen'), - ExecuteProcess( - cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], - output='screen'), + # ExecuteProcess( + # cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], + # output='screen'), IncludeLaunchDescription( PythonLaunchDescriptionSource([launch_file_dir, '/confbot_state_publisher.launch.py']), From d7695e795228e443bdcb4da2d6a313346cd90f4f Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Sat, 4 May 2019 12:24:50 +0200 Subject: [PATCH 12/25] Revert "dont set use_sim_time as currently doesn't impact time used" This reverts commit b79ca27fe72c74bf2c06e012c26e488e419ecee2. --- confbot_simulation/launch/empty_world.launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py index 9695412..f07ddff 100644 --- a/confbot_simulation/launch/empty_world.launch.py +++ b/confbot_simulation/launch/empty_world.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='False') + use_sim_time = LaunchConfiguration('use_sim_time', default='True') world = os.path.join( get_package_share_directory('confbot_simulation'), 'world', 'basic_world.world') launch_file_dir = os.path.join(get_package_share_directory('confbot_bringup'), 'launch') @@ -34,9 +34,9 @@ def generate_launch_description(): cmd=['gazebo', '--verbose', world, '-s', 'libgazebo_ros_factory.so'], output='screen'), - # ExecuteProcess( - # cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], - # output='screen'), + ExecuteProcess( + cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], + output='screen'), IncludeLaunchDescription( PythonLaunchDescriptionSource([launch_file_dir, '/confbot_state_publisher.launch.py']), From 7742dc26b69f59c85c39469003904611945d2a4b Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Sat, 4 May 2019 12:37:08 +0200 Subject: [PATCH 13/25] use libgazebo_ros_init to have sim time published Signed-off-by: Mikael Arguedas --- confbot_simulation/launch/empty_world.launch.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py index f07ddff..992e52a 100644 --- a/confbot_simulation/launch/empty_world.launch.py +++ b/confbot_simulation/launch/empty_world.launch.py @@ -31,7 +31,10 @@ def generate_launch_description(): launch_file_dir = os.path.join(get_package_share_directory('confbot_bringup'), 'launch') return LaunchDescription([ ExecuteProcess( - cmd=['gazebo', '--verbose', world, '-s', 'libgazebo_ros_factory.so'], + cmd=[ + 'gazebo', '--verbose', world, + '-s', 'libgazebo_ros_factory.so', '-s', 'libgazebo_ros_init.so' + ], output='screen'), ExecuteProcess( From d5d09f414d551510d87084ae9911bc514c87ed83 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 20 May 2019 11:17:48 -0700 Subject: [PATCH 14/25] fix conversion error from string to bool Signed-off-by: Karsten Knese --- confbot_bringup/launch/confbot_state_publisher.launch.py | 6 +++--- confbot_simulation/confbot_simulation/urdf_spawner.py | 0 confbot_simulation/launch/empty_world.launch.py | 5 +++-- 3 files changed, 6 insertions(+), 5 deletions(-) mode change 100644 => 100755 confbot_simulation/confbot_simulation/urdf_spawner.py diff --git a/confbot_bringup/launch/confbot_state_publisher.launch.py b/confbot_bringup/launch/confbot_state_publisher.launch.py index ecf0419..e13cf7f 100644 --- a/confbot_bringup/launch/confbot_state_publisher.launch.py +++ b/confbot_bringup/launch/confbot_state_publisher.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='false') + use_sim_time = LaunchConfiguration('use_sim_time', default=True) urdf_file_name = 'confbot.urdf' urdf = os.path.join(get_package_share_directory('confbot_description'), 'urdf', urdf_file_name) @@ -32,7 +32,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( 'use_sim_time', - default_value='false', + default_value=('True' if use_sim_time else 'False'), description='Use simulation (Gazebo) clock if true'), Node( @@ -40,6 +40,6 @@ def generate_launch_description(): node_executable='robot_state_publisher', node_name='robot_state_publisher', output='screen', - parameters=[{'use_sim_time': use_sim_time}], + parameters=[{'use_sim_time': (True if use_sim_time else False)}], arguments=[urdf]), ]) diff --git a/confbot_simulation/confbot_simulation/urdf_spawner.py b/confbot_simulation/confbot_simulation/urdf_spawner.py old mode 100644 new mode 100755 diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py index 992e52a..cbc6acc 100644 --- a/confbot_simulation/launch/empty_world.launch.py +++ b/confbot_simulation/launch/empty_world.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='True') + use_sim_time = LaunchConfiguration('use_sim_time', default=True) world = os.path.join( get_package_share_directory('confbot_simulation'), 'world', 'basic_world.world') launch_file_dir = os.path.join(get_package_share_directory('confbot_bringup'), 'launch') @@ -38,7 +38,8 @@ def generate_launch_description(): output='screen'), ExecuteProcess( - cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], + cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', + ('True' if use_sim_time else 'False')], output='screen'), IncludeLaunchDescription( From 5427e4d19de4ade7947b1d842101cd62a821a9cf Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 20 May 2019 15:10:20 -0700 Subject: [PATCH 15/25] actually using the sim time parameter correctly Signed-off-by: Karsten Knese --- .../launch/confbot_state_publisher.launch.py | 20 ++++++++++++++----- .../launch/empty_world.launch.py | 16 +++++++++++---- 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/confbot_bringup/launch/confbot_state_publisher.launch.py b/confbot_bringup/launch/confbot_state_publisher.launch.py index e13cf7f..7199635 100644 --- a/confbot_bringup/launch/confbot_state_publisher.launch.py +++ b/confbot_bringup/launch/confbot_state_publisher.launch.py @@ -19,20 +19,20 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.conditions.if_condition import IfCondition +from launch.conditions.unless_condition import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default=True) urdf_file_name = 'confbot.urdf' - urdf = os.path.join(get_package_share_directory('confbot_description'), 'urdf', urdf_file_name) return LaunchDescription([ DeclareLaunchArgument( 'use_sim_time', - default_value=('True' if use_sim_time else 'False'), + default_value='True', description='Use simulation (Gazebo) clock if true'), Node( @@ -40,6 +40,16 @@ def generate_launch_description(): node_executable='robot_state_publisher', node_name='robot_state_publisher', output='screen', - parameters=[{'use_sim_time': (True if use_sim_time else False)}], - arguments=[urdf]), + parameters=[{'use_sim_time': True}], + arguments=[urdf], + condition=IfCondition(LaunchConfiguration('use_sim_time'))), + + Node( + package='robot_state_publisher', + node_executable='robot_state_publisher', + node_name='robot_state_publisher', + output='screen', + parameters=[{'use_sim_time': False}], + arguments=[urdf], + condition=UnlessCondition(LaunchConfiguration('use_sim_time'))) ]) diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py index cbc6acc..769bb9c 100644 --- a/confbot_simulation/launch/empty_world.launch.py +++ b/confbot_simulation/launch/empty_world.launch.py @@ -18,6 +18,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument from launch.actions import ExecuteProcess from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -25,11 +26,16 @@ def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default=True) + use_sim_time = False world = os.path.join( get_package_share_directory('confbot_simulation'), 'world', 'basic_world.world') launch_file_dir = os.path.join(get_package_share_directory('confbot_bringup'), 'launch') + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value=str(use_sim_time), + description='Use simulation (Gazebo) clock if true'), ExecuteProcess( cmd=[ 'gazebo', '--verbose', world, @@ -38,12 +44,14 @@ def generate_launch_description(): output='screen'), ExecuteProcess( - cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', - ('True' if use_sim_time else 'False')], + cmd=[ + 'ros2', 'param', 'set', '/gazebo', + 'use_sim_time', LaunchConfiguration('use_sim_time') + ], output='screen'), IncludeLaunchDescription( PythonLaunchDescriptionSource([launch_file_dir, '/confbot_state_publisher.launch.py']), - launch_arguments={'use_sim_time': use_sim_time}.items(), + launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(), ), ]) From 739006f9e73cb6db3a2d60986305f5ac86daa3bf Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 21 May 2019 10:39:42 -0700 Subject: [PATCH 16/25] interpret launch configuration as yaml Signed-off-by: Karsten Knese --- .../launch/confbot_state_publisher.launch.py | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/confbot_bringup/launch/confbot_state_publisher.launch.py b/confbot_bringup/launch/confbot_state_publisher.launch.py index 7199635..1069ba6 100644 --- a/confbot_bringup/launch/confbot_state_publisher.launch.py +++ b/confbot_bringup/launch/confbot_state_publisher.launch.py @@ -19,8 +19,6 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions.if_condition import IfCondition -from launch.conditions.unless_condition import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -40,16 +38,6 @@ def generate_launch_description(): node_executable='robot_state_publisher', node_name='robot_state_publisher', output='screen', - parameters=[{'use_sim_time': True}], - arguments=[urdf], - condition=IfCondition(LaunchConfiguration('use_sim_time'))), - - Node( - package='robot_state_publisher', - node_executable='robot_state_publisher', - node_name='robot_state_publisher', - output='screen', - parameters=[{'use_sim_time': False}], - arguments=[urdf], - condition=UnlessCondition(LaunchConfiguration('use_sim_time'))) + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments=[urdf]) ]) From ae5f96192fa4e094efd322f17bbc40ae322dfb88 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 21 May 2019 17:53:18 -0700 Subject: [PATCH 17/25] visualize the laser scan in gazebo Signed-off-by: Karsten Knese --- confbot_description/urdf/confbot.urdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/confbot_description/urdf/confbot.urdf b/confbot_description/urdf/confbot.urdf index 7e14170..029753f 100644 --- a/confbot_description/urdf/confbot.urdf +++ b/confbot_description/urdf/confbot.urdf @@ -203,7 +203,7 @@ - false + true 3 From 394521eb3281be5ecea0403e3c6530b5a176f588 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 21 May 2019 17:53:29 -0700 Subject: [PATCH 18/25] use sim time Signed-off-by: Karsten Knese --- confbot_simulation/launch/empty_world.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py index 769bb9c..90bc1b3 100644 --- a/confbot_simulation/launch/empty_world.launch.py +++ b/confbot_simulation/launch/empty_world.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): - use_sim_time = False + use_sim_time = True world = os.path.join( get_package_share_directory('confbot_simulation'), 'world', 'basic_world.world') launch_file_dir = os.path.join(get_package_share_directory('confbot_bringup'), 'launch') From e885f50126a4ebee8c3391b2059e214265b421cd Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Wed, 5 Jun 2019 12:50:35 +0200 Subject: [PATCH 19/25] make laser_frame point forward Signed-off-by: Mikael Arguedas --- confbot_description/urdf/confbot.urdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/confbot_description/urdf/confbot.urdf b/confbot_description/urdf/confbot.urdf index 029753f..c55bd85 100644 --- a/confbot_description/urdf/confbot.urdf +++ b/confbot_description/urdf/confbot.urdf @@ -32,7 +32,7 @@ - + From 6777de9d4b343446822dc40aeb7c1a19a2420e36 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Wed, 5 Jun 2019 14:42:43 +0200 Subject: [PATCH 20/25] gazebo_ros now declares/set use_sim_time to true by default Signed-off-by: Mikael Arguedas --- confbot_simulation/launch/empty_world.launch.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/confbot_simulation/launch/empty_world.launch.py b/confbot_simulation/launch/empty_world.launch.py index 90bc1b3..825b8a8 100644 --- a/confbot_simulation/launch/empty_world.launch.py +++ b/confbot_simulation/launch/empty_world.launch.py @@ -43,13 +43,6 @@ def generate_launch_description(): ], output='screen'), - ExecuteProcess( - cmd=[ - 'ros2', 'param', 'set', '/gazebo', - 'use_sim_time', LaunchConfiguration('use_sim_time') - ], - output='screen'), - IncludeLaunchDescription( PythonLaunchDescriptionSource([launch_file_dir, '/confbot_state_publisher.launch.py']), launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(), From 63cfa777431cf9163afbfb0a58a985facc36cc40 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Wed, 5 Jun 2019 15:29:57 +0200 Subject: [PATCH 21/25] add camera Need to create an optical frame to match REP103 as camera plugin uses x forward and not z forward Need to source Gazebo setup file manually for the camera plugin to be found and loaded successfully No namespace provided to plugin as it appends the sensor name to the namespace already, otherwise results in /confbot_camera/confobt_camera/image_raw Signed-off-by: Mikael Arguedas --- confbot_description/urdf/confbot.urdf | 60 ++++++++++++++++++++++++++- 1 file changed, 59 insertions(+), 1 deletion(-) diff --git a/confbot_description/urdf/confbot.urdf b/confbot_description/urdf/confbot.urdf index c55bd85..5b7f216 100644 --- a/confbot_description/urdf/confbot.urdf +++ b/confbot_description/urdf/confbot.urdf @@ -57,6 +57,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + @@ -236,5 +261,38 @@ - + + + 30 + true + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + camera_optical_frame + + + + + + + + From ea88a55ea8aa135811d71c437b8e82090265c01d Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Wed, 5 Jun 2019 16:55:57 +0200 Subject: [PATCH 22/25] move back camera_frame on the robot base Signed-off-by: Mikael Arguedas --- confbot_description/urdf/confbot.urdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/confbot_description/urdf/confbot.urdf b/confbot_description/urdf/confbot.urdf index 5b7f216..7359232 100644 --- a/confbot_description/urdf/confbot.urdf +++ b/confbot_description/urdf/confbot.urdf @@ -58,7 +58,7 @@ - + From 18e5594f56687857915b9184070fb1092a2845c5 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Thu, 6 Jun 2019 00:08:32 +0200 Subject: [PATCH 23/25] add rviz config for gazebo Signed-off-by: Mikael Arguedas --- confbot_simulation/CMakeLists.txt | 2 +- confbot_simulation/config/config_gazebo.rviz | 204 +++++++++++++++++++ 2 files changed, 205 insertions(+), 1 deletion(-) create mode 100644 confbot_simulation/config/config_gazebo.rviz diff --git a/confbot_simulation/CMakeLists.txt b/confbot_simulation/CMakeLists.txt index 36dd53f..107dad3 100644 --- a/confbot_simulation/CMakeLists.txt +++ b/confbot_simulation/CMakeLists.txt @@ -4,7 +4,7 @@ project(confbot_simulation) # find dependencies find_package(ament_cmake REQUIRED) -install(DIRECTORY launch world +install(DIRECTORY config launch world DESTINATION share/${PROJECT_NAME}) install(PROGRAMS ${PROJECT_NAME}/urdf_spawner.py DESTINATION lib/${PROJECT_NAME}) diff --git a/confbot_simulation/config/config_gazebo.rviz b/confbot_simulation/config/config_gazebo.rviz new file mode 100644 index 0000000..d6f97e4 --- /dev/null +++ b/confbot_simulation/config/config_gazebo.rviz @@ -0,0 +1,204 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /LaserScan1 + - /Image1 + Splitter Ratio: 0.38277512788772583 + Tree Height: 587 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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 + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + camera_frame: + Value: true + camera_optical_frame: + Value: true + laser_frame: + Value: true + odom: + Value: true + wheel_l_link: + Value: true + wheel_r_link: + Value: true + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + odom: + base_link: + camera_frame: + camera_optical_frame: + {} + laser_frame: + {} + wheel_l_link: + {} + wheel_r_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: /Users/karsten/workspace/osrf/roscon2018_ws/src/roscon2018/confbot_description/urdf/confbot.urdf + Description Source: File + Description Topic: /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: "" + Unreliable: false + Update Interval: 0 + Value: true + Visual Enabled: 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 + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /scan + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Queue Size: 10 + Topic: /danger_zone + Unreliable: false + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Queue Size: 10 + Topic: /safe_zone + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 10 + Topic: /confbot_camera/image_raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: laser_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 13.815640449523926 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 2.1995294970601542e-18 + Y: -1.4232764294392875e-39 + Z: 2.2182833359079564e-18 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5297963619232178 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1467127799987793 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000275000003a7fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000304000000ce0000000000000000fb0000000a0049006d0061006700650100000319000000cb0000002800ffffff000000010000010f000003a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c000003a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c2000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 From 82127e10ca913db9167c359ac4e746a919ff13c1 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Thu, 6 Jun 2019 00:09:32 +0200 Subject: [PATCH 24/25] add preliminary Gazebo section Signed-off-by: Mikael Arguedas --- tutorial/Linux_Tutorial.md | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/tutorial/Linux_Tutorial.md b/tutorial/Linux_Tutorial.md index 7bba785..3a1fec9 100644 --- a/tutorial/Linux_Tutorial.md +++ b/tutorial/Linux_Tutorial.md @@ -386,3 +386,34 @@ $ python3 safe_zone_publisher.py ``` Here it fails with `rt/cmd_vel topic not found in allow rule.` meaning that the not is not allowed to publish on the ROS topic `/cmd_vel`. + + +##### Simulating the robot in Gazebo + +Make sure to kill all the running nodes. + +Shell 1: Set up Gazebo environment variables and launch the simulation +```bash +source /usr/share/gazebo/setup.sh +export GAZEBO_MODEL_PATH=$CONFBOT_WS/install/share/:$GAZEBO_MODEL_PATH +ros2 launch confbot_simulation empty_world.launch.py +``` + +Shell 2: Spawn our robot in Gazebo +```bash +ros2 run confbot_simulation urdf_spawner.py `ros2 pkg prefix confbot_description --share`/urdf/confbot.urdf +``` +Then we give a velocity command to our robot +```bash +ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{angular: {z: 1}}" -1 +``` + +Shell 3: monitor our robot with RViz +```bash +ros2 run rviz2 rviz2 -d `ros2 pkg prefix confbot_simulation --share`/config/confbot.rviz +``` + +TODO: remove following lines and place them in an RViz config file +Toggle off and on the `Image` plugin + +We can see our robot using a differential drive controller, a simulated laser scanner and a simulated camera! From cadb71a6d3571a6e411f53964de7881c54261cc3 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Thu, 6 Jun 2019 00:17:47 +0200 Subject: [PATCH 25/25] fix config file name in instructions Signed-off-by: Mikael Arguedas --- tutorial/Linux_Tutorial.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorial/Linux_Tutorial.md b/tutorial/Linux_Tutorial.md index 3a1fec9..533250a 100644 --- a/tutorial/Linux_Tutorial.md +++ b/tutorial/Linux_Tutorial.md @@ -410,7 +410,7 @@ ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{angular: {z: 1}}" -1 Shell 3: monitor our robot with RViz ```bash -ros2 run rviz2 rviz2 -d `ros2 pkg prefix confbot_simulation --share`/config/confbot.rviz +ros2 run rviz2 rviz2 -d `ros2 pkg prefix confbot_simulation --share`/config/config_gazebo.rviz ``` TODO: remove following lines and place them in an RViz config file